Including a class definition, within a sketch which contains 'standard' commands

Hi there,

I was hoping somebody might be able to help out getting a final function complete on my robot project. It’s my first arduino project and has been going quite well so far given my lack of experience!

I’m currently building a robot controlled via a PS3 bluetooth controller, and have the sketch for it completed except for one function. The design calls for a DC motor to be used as a servo, in order to move a heavy pendulum weight from side to side, using one of the ‘joysticks’ on the controller. A regular servo would not work due to the heavy weight being used (it’s a counter weight).

I have found a code which uses a class definition (customServo) and uploaded that, and the sketch seems to work in it’s original form OK, that is, it can sweep my DC motor from side to side using a pot attached for feedback.

However… when I try and incorporate this code into my existing (working) code for the robot, all the existing code ceases working at all, and all that remains working is the newly incorporated ‘sweep’.

The existing code controls three servos, a ‘normal’ DC motor using PWM signals, and a range of sound files held on another arduino.

My hardware is as follows:

Arduino Mega running the main code, with a USB shield and Bluetooth Dongle attached via tx/rx to:
Arduino Uno running a simple MP3 player sketch, triggered via serial commands from the Mega’s sketch.

So… what am I trying to do? :- Incorporate code which allows me to control a DC motor in the same way as a servo, using the PS3 controller commands, and a potentiometer as feedback.

Here is the working code so far

/*
Matthew Reading
December 2017
This is a modified version of the BT_RC_Trim.ino sketch by Barrett Anderies
For more information visit my facebook page www.facebook.com/wellingtonironman
*/

#include <PS3BT.h> //Include the necessary libraries.
#include <Servo.h>
#include <SPI.h>

USB Usb;
BTD Btd(&Usb);
PS3BT PS3(&Btd); 

Servo servo1; //Create instances of type Servo. servo1 is the dome rotate servo and servo2 and servo3 is the head tilt.
Servo servo2;
Servo servo3;

#define pwmA 6
#define pwmB 7

int motorSpeedFwd = 0;
int motorSpeedRev = 0;

void setup() {
Serial.begin(115200); 
if (Usb.Init() == -1) { 
Serial.print(F("\r\nOSC did not start"));
while(1); //halt
} 
pinMode(3, OUTPUT);
pinMode(5, OUTPUT);
pinMode(4, OUTPUT);
pinMode(pwmA, OUTPUT);
pinMode(pwmB, OUTPUT);
servo1.attach(5); //Dome rotate servo on digital pin 5
servo2.attach(3); //head tilt right side on digital pin 3
servo3.attach(4); //head tilt left side on digital pin 4
}
void loop() 
{
Usb.Task();

if(PS3.PS3Connected || PS3.PS3NavigationConnected) {

servo1.write(map(PS3.getAnalogHat(RightHatX), 0, 255, 180, 0)); // write dome steer command
servo2.write(map(PS3.getAnalogHat(RightHatY), 0, 255, 0, 135)); // write dome tilt command
servo3.write(map(PS3.getAnalogHat(RightHatY), 0, 255, 180, 45));

if (PS3.getButtonClick(L1))
Serial.print(F("\r\n1"));
if (PS3.getButtonClick(R1))
Serial.print(F("\r\n2"));
if (PS3.getButtonClick(L3))
Serial.print(F("\r\n3"));
if (PS3.getButtonClick(UP))
Serial.print(F("\r\n4"));
if (PS3.getButtonClick(R3))
Serial.print(F("\r\n5"));
if (PS3.getButtonClick(CIRCLE))
Serial.print(F("\r\n+++++++++++++++++++6"));
if (PS3.getButtonClick(DOWN))
Serial.print(F("\r\n7"));
if (PS3.getButtonClick(RIGHT))
Serial.print(F("\r\n8"));
if (PS3.getButtonClick(LEFT))
Serial.print(F("\r\n9"));
{
if (PS3.getAnalogHat(LeftHatY) < 99) {
motorSpeedFwd = map(PS3.getAnalogHat(LeftHatY), 99, 0, 0, 255);
}
if (PS3.getAnalogHat(LeftHatY) > 155) {
motorSpeedRev = map(PS3.getAnalogHat(LeftHatY), 155, 255, 0, 255);
}
if (PS3.getAnalogHat(LeftHatY) > 100 && PS3.getAnalogHat(LeftHatY) < 154) {
motorSpeedRev = 0;
motorSpeedFwd = 0;
}

analogWrite(pwmA, motorSpeedFwd); // Send PWM signal to motor driver Fwd
analogWrite(pwmB, motorSpeedRev); // Send PWM signal to motor driver Rev 
}
if (PS3.getButtonClick(PS))
PS3.disconnect();

}
else
{
servo1.write(90);
servo2.write(90);
servo3.write(90);
}
}

You need to show us the attempt at integration.

Sorry, forum wouldnt let me post twice in succession. Heres the attempt I made.

Additional, probably relevant information. The pot is of course connected to the DC motor output shaft so it can read the position. The motor is geared down via a worm gear and is only about 40rpm max, so should be able to hold a position relatively well and get there nice and slowly by using lower pwm values if needed. The DC motor is a 12v car window motor, and uses a BTS 7960 driver.

I’m fully aware that there are probably easier ways of achieving my end goal of a DC motor as a servo using pot feedback, but my coding skills don’t extend as far as being able to write from scratch, so I’m restricted to finding examples online and trying to re-purpose them. It’s been working so far!

/*
Matthew Reading
December 2017
This is a modified version of the BT_RC_Trim.ino sketch by Barrett Anderies
For more information visit my facebook page www.facebook.com/wellingtonironman
*/

#include <PS3BT.h> //Include the necessary libraries.
#include <Servo.h>
#include <SPI.h>

USB Usb;
BTD Btd(&Usb);
PS3BT PS3(&Btd); 

Servo servo1; //Create instances of type Servo. servo1 is the dome rotate servo and servo2 and servo3 is the head tilt.
Servo servo2;
Servo servo3;

#define pwmA 6
#define pwmB 7
#define POT_VALUE_MAX 700 
#define POT_VALUE_MIN 200
#define PERM_ERROR 3 
#define MAX_ANGLE 180 

int motorSpeedFwd = 0;
int motorSpeedRev = 0;
int potPin = A5; //we will read the potentiometer value on analog pin 5
int motor_p1 = 11;
int motor_p2 = 12;
int pwmPin = 2;
int turnSpeed = 255;


class DCMotor
{
private:

    int M_pin1 , M_pin2, M_PWMPin;

    int M_Speed;

    int turnDirection;
    
    enum turnDirection {right, left};

public:

    DCMotor(int p1, int p2, int p3) //Constructor
    {
            M_pin1 = p1; //direction pin on L298

            M_pin2 = p2;  //direction pin on L298

            M_PWMPin = p3;  //PWM pin on L298
            
            pinMode(M_pin1, OUTPUT);
            
            pinMode(M_pin2, OUTPUT);
    }

    void SetTurnDirection(int dir)//Setting turn directions on L298 IC
    {
        turnDirection = dir;

        switch(turnDirection)
        {
            case right: //turning Right

                    //motor moves CW

                    digitalWrite(M_pin1, HIGH);
                    digitalWrite(M_pin2, LOW);

                    break;

            case left: //turning Left

                    //motor moves CCW

                    digitalWrite(M_pin1, LOW);
                    digitalWrite(M_pin2, HIGH);

                  break;
        }
    }

    void SetTurnSpeed(int s)
    {
         M_Speed = s;
    }
    
    void Turn()
    {
        analogWrite(M_PWMPin, M_Speed);
    }
    
    void Stop()
    {
        analogWrite(M_PWMPin, 0);
    }
    
    void GoToAngle(int target, int howFast)
    {
      //find out the current angle of the motor shaft
  
      int currentAngle = ((float)analogRead(potPin) - POT_VALUE_MIN)/(POT_VALUE_MAX - POT_VALUE_MIN) * MAX_ANGLE;
      
      //First Check if we need to turn left or right.....
      if (currentAngle < target)
      {
        SetTurnDirection(right);
      } 
      else if (currentAngle > target)
      {
        SetTurnDirection(left);
      }
      
      SetTurnSpeed(howFast);
      
      while(abs(currentAngle - target) > PERM_ERROR)//if the shaft is not at the required value,
      {
        Turn();//Keep on turning the shaft
        
        //Allow the motor to turn a little and wait here for a moment...
        
        delay(100); //adjust the delay as required depending on your motor speed
        
        //update the current angle of the shaft 
        
        currentAngle = ((float)analogRead(potPin) - POT_VALUE_MIN)/(POT_VALUE_MAX - POT_VALUE_MIN) * MAX_ANGLE;
      } 
      
      Stop(); //Stop the shaft after the error is acceptable
    }

};

//DC Motor Class definition completed................

DCMotor customServo(motor_p1, motor_p2, pwmPin); //create an instance of the DC motor Class

void setup() {
Serial.begin(115200); 
if (Usb.Init() == -1) { 
Serial.print(F("\r\nOSC did not start"));
while(1); //halt
} 
pinMode(3, OUTPUT);
pinMode(5, OUTPUT);
pinMode(4, OUTPUT);
pinMode(pwmA, OUTPUT);
pinMode(pwmB, OUTPUT);
servo1.attach(5); //Dome rotate servo on digital pin 5
servo2.attach(3); //head tilt right side on digital pin 3
servo3.attach(4); //head tilt left side on digital pin 4
}
void loop() 
{
Usb.Task();

if(PS3.PS3Connected || PS3.PS3NavigationConnected) {

servo1.write(map(PS3.getAnalogHat(RightHatX), 0, 255, 180, 0)); // write dome steer command
servo2.write(map(PS3.getAnalogHat(RightHatY), 0, 255, 0, 135)); // write dome tilt command
servo3.write(map(PS3.getAnalogHat(RightHatY), 0, 255, 180, 45));

if (PS3.getButtonClick(L1))
Serial.print(F("\r\n1"));
if (PS3.getButtonClick(R1))
Serial.print(F("\r\n2"));
if (PS3.getButtonClick(L3))
Serial.print(F("\r\n3"));
if (PS3.getButtonClick(UP))
Serial.print(F("\r\n4"));
if (PS3.getButtonClick(R3))
Serial.print(F("\r\n5"));
if (PS3.getButtonClick(CIRCLE))
Serial.print(F("\r\n+++++++++++++++++++6"));
if (PS3.getButtonClick(DOWN))
Serial.print(F("\r\n7"));
if (PS3.getButtonClick(RIGHT))
Serial.print(F("\r\n8"));
if (PS3.getButtonClick(LEFT))
Serial.print(F("\r\n9"));
{
if (PS3.getAnalogHat(LeftHatY) < 99) {
motorSpeedFwd = map(PS3.getAnalogHat(LeftHatY), 99, 0, 0, 255);
}
if (PS3.getAnalogHat(LeftHatY) > 155) {
motorSpeedRev = map(PS3.getAnalogHat(LeftHatY), 155, 255, 0, 255);
}
if (PS3.getAnalogHat(LeftHatY) > 100 && PS3.getAnalogHat(LeftHatY) < 154) {
motorSpeedRev = 0;
motorSpeedFwd = 0;
}

analogWrite(pwmA, motorSpeedFwd); // Send PWM signal to motor driver Fwd
analogWrite(pwmB, motorSpeedRev); // Send PWM signal to motor driver Rev 
}

//forwards turning..........
  for (int i = 30; i <= 150; i += 15) //our motor will turn from 30 to 150 degrees in steps of 15 degrees
  {
    customServo.GoToAngle(i, turnSpeed);
    
  }
  
  //backwards turning..........
  for (int i = 150; i >=30; i-=15)//now turn from 150 to 30 degrees in steps of 15 degrees
  {
    customServo.GoToAngle(i, turnSpeed);
    
  }

if (PS3.getButtonClick(PS))
PS3.disconnect();

}
else
{
servo1.write(90);
servo2.write(90);
servo3.write(90);
}
}

dynofiend:
Sorry, forum wouldnt let me post twice in succession. Heres the attempt I made.

So, your constructor is manipulating hardware:

    DCMotor(int p1, int p2, int p3) //Constructor
    {
            M_pin1 = p1; //direction pin on L298

            M_pin2 = p2;  //direction pin on L298

            M_PWMPin = p3;  //PWM pin on L298
            
            pinMode(M_pin1, OUTPUT);
            
            pinMode(M_pin2, OUTPUT);
    }

but, the hardware is not guaranteed to be ready until setup() is called. Therefore, the pinMode() functions may not behave as expected.

The way this is typically solved is to include a method in your class called begin() or init() where you set the pinMode for the hardware pins with which you are working and merely define them in the constructor.

Thanks very much for the reply.

The problem I have is not that the functions or pins related to the class definition don't work. The opposite in fact; they work but the rest of the code doesn't.

Do you think that your suggestion will have an effect on that?

dynofiend:
Do you think that your suggestion will have an effect on that?

Well, start with taking out the hardware setup from the constructor and making sure that is happening in setup. What you have is just not the way to do it. Note this:

Servo.attach()

look at the source and see what happens there.

Thanks very much. Will try moving that hardware setup into the relevant place and go from there. :slight_smile:

Very much appreciate your time!

      while(abs(currentAngle - target) > PERM_ERROR)//if the shaft is not at the required value,

{
        Turn();//Keep on turning the shaft
       
        //Allow the motor to turn a little and wait here for a moment...
       
        delay(100); //adjust the delay as required depending on your motor speed
       
        //update the current angle of the shaft
       
        currentAngle = ((float)analogRead(potPin) - POT_VALUE_MIN)/(POT_VALUE_MAX - POT_VALUE_MIN) * MAX_ANGLE;
      }

This is BLOCKING CODE. The servo doesn't release control back to the rest of the program until it has arrived at the target position. This must be completely rewritten so that it just starts the motor moving towards the target and then lets the rest of the code run. The entire loop should complete in just a few milliseconds, to give this an opportunity to see if it's reached the target and stop the motor.

Ok thanks. That would explain why it works but nothing else does, as the example just has a continuous sweep.

Unfortunately the complete re-write required is way beyond my ability, so I'll look for another solution elsewhere.

Thank you again for the help.