Micro Serial Servo Controller + Xbee

[Dizzixx]

So I am making a robot for a class and I have two Xbee shields each connected to a different arduino. On one side is the controller and the other the robot. On the robot side I am trying to use the micro serial servo controller to manage servo control. I had to make a software serial port as the hardware one is already being used by the Xbee. The thing is after doing this everything is acting weird.

Before my controller and robot communicated just fine. Now the robot doesnt seem to get signals from the controller. But I can connect it via USB and use the serial monitor to send it commands.

The servo controller is acting even more strange. It was working I could send it an angle (Via USB to the arduino) and it would move the servo, then it would frequently for seemingly no reason go red light on me, I would reset the whole thing and try again. Now for some reason the servo controller has no lights (red, yellow or green) and it just spins the servo round and round endlessly.

What gives?

**** CONTROLLER CODE ****

//Controller Code; Arduino Duemilanove ATmega328

//---PINS---//
//number of digital pins on the Duemilanove used to set analog pin modes
const int DigitalPins = 14;
//LEDS
int RED_LED = 3;
int GREEN_LED = 4;
//Digital pins
const int RunSwitch = 5;//digital pin the run switch is on
const int DrivePush = 6;
const int ClawPush = 8;

//Analog pins
const int FrdPot = 0; //Analog pin that the Forward axis pot is on
const int TrnPot = 1;//Analog pin that the turning axis pot is on

const int RaisePot = 2; //Analog pin that the Raise/Lower axis pot is on
const int OpenPot = 3;//Analog pin that the Open/Close axis pot is on

const int TrimFrdPot = 4; //Analog pin that the Trim Forward axis pot is on
const int TrimTrnPot = 5;//Analog pin that the Trim turning axis pot is on
//---END PINS---//


//---Values---//
int State = 0;//state condition values; run switch high/low
int DELAY = 30; //delay between serial commands in milliseconds
const int DeadSpace = .10;

//Motors
const int POW = 140; //Max PWM power possible is actually 3/4*POW
//Acceptable values range from 0 to 340

//Motor computation variable
//int Frd = 0;
//int Trn = 0;
int TrimF = 0;
int TrimT = 0;
int drive = 0;
int turn = 0;
int LeftM = 0;
int RightM = 0;

//Left motor controls
int LM_IN1_Val = 0;
int LM_IN2_Val = 0;
//Right motor control
int RM_IN1_Val = 0;
int RM_IN2_Val = 0;

//Claw Values default angles at power on.
int Raise = 90;
int Open = 30;
int AngleInc = 3;
//State of claw stick push button
int ClawPushS = 0;
//State of drive stick push button
int DrivePushS = 0;
//Min and Max servo angles
const int MinRaise = 0; 
const int MaxRaise = 130;
const int DefaultRaise = 100;
const int MinOpen = 0;
const int MaxOpen = 130;
const int DefaultOpen = 100;


//--END Values---//


void setup()//the main initialization function
{
  //Pinmode initialization
  pinMode(RunSwitch, INPUT);//the run switch pin
  pinMode(RED_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  pinMode(FrdPot+DigitalPins, INPUT); //Forward axis potentiometer pin
  pinMode(TrnPot+DigitalPins, INPUT);//Turning axis potentiometer pin
  
  Serial.begin(38400); //starts the serial communication at 38400 baud rate
}//end Setup()

void control()
{
  //Reading pushbuttons on each stick
  ClawPushS = digitalRead(ClawPush); //launch command
  
  
  DrivePushS = digitalRead(DrivePush);// resets claw to default position
  if(DrivePushS == 1)
  {
    Raise = DefaultRaise; 
    Open = DefaultOpen;
  }
  
  
  int deadClawStick = 15;
  //Reading Claw Control values and setting corresponding angles. 
    if(map(analogRead(RaisePot),0,1023,-100,100)>deadClawStick)
  
    if(Raise > 180)
    {
      Raise = 180;
    }
    else
    {
      Raise = Raise + AngleInc;
    }
  if(map(analogRead(RaisePot),0,1023,-100,100)<-deadClawStick)
  {
    if(Raise <0)
    {
      Raise = 0;
    }
    else
    {
      Raise = Raise - AngleInc;
    }
  }
  
  if(map(analogRead(OpenPot),0,1023,-100,100)>deadClawStick)
  {
    if(Open > 180)
    {
      Open = 180;
    }
    else
    {
      Open = Open + AngleInc;
    }
  }
   if(map(analogRead(OpenPot),0,1023,-100,100)<-deadClawStick)
  {
    if(Open<0)
    {
      Open = 0; 
    }
    else
    {
      Open = Open -AngleInc;
    }
  }
  
  //Reading the drive control pot values and re mapping to a new scale
  drive = map(analogRead(FrdPot),0,1023,-POW/2,POW/2)+TrimF;
  turn = map(analogRead(TrnPot),0,1023,-POW/4,POW/4)+TrimT;
  
  if (drive <=DeadSpace*POW/2 && drive >= -DeadSpace*POW/2)
  {
   drive = 0; 
  }
  if (turn <=DeadSpace*POW/4 && turn >= -DeadSpace*POW/4)
  {
   turn = 0; 
  }
  //TrimF = map(analogRead(TrimFrdPot),0,1023,-100,100);
  //TrimT = map(analogRead(TrimTrnPot),0,1023,-100,100);
  
  //Raise = map(analogRead(RaisePot),0,1023,-100,100);
  //Open = map(analogRead(OpenPot),0,1023,-100,100);

  
  /* Motor pin breakdown
  Forward = IN1 HIGH, IN2 LOW
  Backwards = IN2 HIGH, IN1 LOW */

  //simple algorithim to achieve driving+turning together
  //drive = map(Frd,-100,100,-POW/2,POW/2)+TrimF;
  //turn = map(Trn,-100,100,-POW/4,POW/4)+TrimT;
    
   LeftM = drive-turn;
   RightM = drive+turn;


    
    if(RightM>0)//right motor forward drive
    {
      //Right Forwards
      RM_IN1_Val = RightM;
      RM_IN2_Val = 0;   
    }
    else if(RightM<0)//right motor reverse
    {
      //Right Reverse
      RM_IN1_Val = 0;
      RM_IN2_Val = abs(RightM);        
    }
    
    if(LeftM>0)//left  forward
    {
      //Left Forwards
      LM_IN1_Val = LeftM;
      LM_IN2_Val = 0;    
    }
    else if(LeftM<0)//Left Reverse
    {
      //Right Reverse
      LM_IN1_Val = 0;
      LM_IN2_Val = abs(LeftM);      
    }
    
    
    

}//end control()



void com()
{
 //USED LETTERS
 //
 // x - end command char
 // S - State condition switch
 // Q - LM_IN1 Value
 // W - LM_IN2 Value
 // E - RM_IN1 Value
 // R - RM_IN2 Value
 // O - Claw; open close set angle
 // U - Claw; raise lower set angle
 // L - Launch Solenoid state
 
  
  //STATE CONDITION VALUE
  Serial.print('S');//State condition switch value character
  Serial.print(State);//State condition switch value
  Serial.print('x');//end command character
  delay(DELAY);
  
  //LEFT MOTOR VALUES
  Serial.print('Q');//LM_IN1 value character
  Serial.print(LM_IN1_Val);
  Serial.print('x');//end command character
  delay(DELAY);//delay to ensure the commands are recieved seperately
  
  Serial.print('W');//LM_IN2 value character
  Serial.print(LM_IN2_Val);
  Serial.print('x');//end command character
  delay(DELAY);//delay to ensure the commands are recieved seperately  

  //RIGHT MOTOR VALUES
  Serial.print('E');//RM_IN1 value character
  Serial.print(RM_IN1_Val);
  Serial.print('x');//end command character
  delay(DELAY);//delay to ensure the commands are recieved seperately
  
  Serial.print('R');//RM_IN2 value character
  Serial.print(RM_IN2_Val);
  Serial.print('x');//end command character
  delay(DELAY);//delay to ensure the commands are recieved seperately
  
  //CLAW POSITION VALUES
  Serial.print('O');//Open/close claw value character
  Serial.print(Open);
  Serial.print('x');//end command character
  delay(DELAY);//delay to ensure the commands are recieved seperately
  
  Serial.print('U');//Raise/Lower claw value character
  Serial.print(Raise);
  Serial.print('x');//end command character
  delay(DELAY);//delay to ensure the commands are recieved seperately
  
  //Launch State
  Serial.print('L');//Open/close claw value character
  Serial.print(ClawPushS);
  Serial.print('x');//end command character
  delay(DELAY);//delay to ensure the commands are recieved seperately

  Serial.print('\n');
  
}//end Com()


void loop()
{
  State = digitalRead(RunSwitch);//checking the run state first
  //Serial.print(State);
  //Serial.print('\n');
  
  control();//running the control function
  
  
  //LED status indicator
  if(State == 1)
  {
    digitalWrite(GREEN_LED, HIGH);
    digitalWrite(RED_LED,LOW);
    com();//running serial communication function
  }
  if(State == 0)
  {
    digitalWrite(GREEN_LED, LOW);
    digitalWrite(RED_LED,HIGH);
     LM_IN1_Val = 0;
     LM_IN2_Val = 0;
     RM_IN1_Val = 0;
     RM_IN2_Val = 0;
     com();//running serial communication function
        
  }

}//end Void loop()

**** ROBOT CODE****

//Reciever Code; Arduino Mega (ATmega1280)

/*
A large portion of the code for the serial communication with the servo controller was borrowed from Hannes Hassler who himself 
credited Tom Igoe, David Mellis and Heather Dewey-Hagborg and individuals that were active in the thread

'https://forum.pololu.com/t/arduino-pololu-micro-serial-8-servo-controller/613/1&highlight=arduino'
discussing mostly between Adam, Phil and Zagrophyte
  
Code sourced from 
https://forum.pololu.com/t/arduino-and-micro-serial-servo-controller/993/1
*/

#include <SoftwareSerial.h>
#include <math.h>//include the floating point math library
#include <Servo.h>//include the Servo library

//---PINS---//
//LEDS
const int RED_LED = 34;
const int GREEN_LED = 30;

//Servo numbers on pololu board
const int RaiseServo = 1;
const int OpenServo = 2;
//Serial com with driver
#define reset 13
#define rxPin 12
#define txPin 11
//spoofing a serial port
SoftwareSerial mySerial =  SoftwareSerial(rxPin, txPin);


//MOTOR DRIVER PINS
//Forward = IN1 HIGH, IN2 LOW
//Backwards = IN2 HIGH, IN1 LOW 
//Left Motor
const int LM_IN1 = 7;
const int LM_IN2 = 6;
//Right Motor
const int RM_IN1 = 5;
const int RM_IN2 = 4;

//---END PINS---//


//---Values---//
//State position
int State = 0;
int Debug = 0;
//Drive values
int LM_IN1_Val = 0;
int LM_IN2_Val = 0;
int RM_IN1_Val = 0;
int RM_IN2_Val = 0;
//Claw Values
int Raise_Ang = 0;
int Open_Ang = 0;
//Launch State
int Launch = 0;

//--- END Values---//

void setup()//main initialization function
{
  //Servo controller serial communication initialization
  // define pin modes for tx, rx, led pins:
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  pinMode(reset, OUTPUT);  
  //Reset the Pololu Servo controller
  resetPololu();
  //Begin mySerial at a baud rate of 9600
  mySerial.begin(9600);
  
  //pinmode initialization for status indicating LEDs
  pinMode(RED_LED, OUTPUT);
  pinMode(GREEN_LED, OUTPUT);
  
  //Pinmode initialization for motor driver
  //Left
  pinMode(LM_IN1, OUTPUT);
  pinMode(LM_IN2, OUTPUT);
  //Right
  pinMode(RM_IN1, OUTPUT);
  pinMode(RM_IN2, OUTPUT);
  
  //Pinmode
  
  //Serial communication initialization
  Serial.begin(38400); //starts the serial communication at 38400 baud rate
}

void servos()
{
  send2Pololu(RaiseServo,Raise_Ang); //Open close servo
  send2Pololu(OpenServo,Open_Ang); //Raise lower servo
}

void resetPololu() 
{
  digitalWrite(reset, LOW);
  delay(100);
  digitalWrite(reset,HIGH);
}//END resetPololu

void send2Pololu(int servo, int IntAngle)
{
  int angle = map(IntAngle,0,180,500,5500);
  
  unsigned char buff[6];

  unsigned int temp;
  unsigned char pos_hi,pos_low;

  //Convert the angle data into two 7-bit bytes
  temp=angle&0x1f80;
  pos_hi=temp>>7;
  pos_low=angle & 0x7f;

  //Construct a Pololu Protocol command sentence
  buff[0]=0x80; //start byte
  buff[1]=0x01; //device id
  buff[2]=0x04; //command number
  buff[3]=servo; //servo number
  buff[4]=pos_hi; //data1
  buff[5]=pos_low; //data2
  
  
  //Send the command to the servo controller
  for(int i=0;i<6;i++){
   mySerial.print(buff[i]);  
  }  
}//End send2Pololu

void servoSetSpeed(int servo, int speed)
{
   //servo is the servo number (typically 0-7)
   //speed is servo speed 
   //(0= no speed limiting, 1=slowest, 127=fastest)

   unsigned char buff[5];
   unsigned char speedcmd;
   
   speedcmd=speed&0x7f;//take only lower 7 bits of speed

   buff[0]=0x80;//start byte
   buff[1]=0x01;//device id
   buff[2]=0x01;//command number
   buff[3]=servo;//servo number
   buff[4]=speed;//data1

   for(int i=0;i<5;i++){
      Serial.print(buff[i],BYTE);
   }  
}

void servoOff(unsigned char servo)
{  //turns off a servo
   //(servo will go limp until next position command)
   //servo is the servo number (typically 0-7)

   //Send a Pololu Protocol command
   Serial.print(0x80,BYTE);//start byte
   Serial.print(0x01,BYTE);//device id
   Serial.print(0x00,BYTE);//command number
   Serial.print(servo,BYTE);//servo number
   Serial.print(0x0f,BYTE);//data1 (turn servo off, keep full range) 
}

void com()//communication function used for serial communication between arduinos
{ 

  
  int val = 0;//variable used to track char to number values
  
  if(Serial.available()>0)//if Serial data is in the buffer
  {
    int ByteSize = Serial.available();//determining the size of incoming data
    char data[ByteSize];//initializing the data[] variable
    
    for(int i=0; i<ByteSize; i++)//populating the data array
    {
     data[i] = Serial.read();
    }
    
    //DONE READING INPUT DECODE AND SET VALUES
    //
    // x - end command char
    // D - Debug command
    // S - State condition switch
    // Q - LM_IN1 Value
    // W - LM_IN2 Value
    // E - RM_IN1 Value
    // R - RM_IN2 Value
    // O - Claw; open close set angle
    // U - Claw; raise lower set angle
    // L - Launch Solenoid state
    
   if(data[0] == 'D')//Debug command outputs values via serial
  {
   int power; //tracking the power of ten that any given int has
   for(int i=1; data[i]!='x' ;i++)//for all bytes not the end byte
    {
      power = ByteSize-2-i;//power of ten
      val = val+(data[i]-48)*pow(10,power);//converting the bytes from char to int
    } 
    Debug = val;
   }//end if 'S'      
    
   if(data[0] == 'S')//State condition switch
  {
   int power; //tracking the power of ten that any given int has
   for(int i=1; data[i]!='x' ;i++)//for all bytes not the end byte
    {
      power = ByteSize-2-i;//power of ten
      val = val+(data[i]-48)*pow(10,power);//converting the bytes from char to int
    } 
    State = val;
   }//end if 'S'   

   if(data[0] == 'Q')//LM_IN1 value
  {
   int power; //tracking the power of ten that any given int has
   for(int i=1; data[i]!='x' ;i++)//for all bytes not the end byte
    {
      power = ByteSize-2-i;//power of ten
      val = val+(data[i]-48)*pow(10,power);//converting the bytes from char to int
    } 
    LM_IN1_Val = val;
   }//end if 'Q'     
    
   if(data[0] == 'W')//LM_IN2 value
  {
   int power; //tracking the power of ten that any given int has
   for(int i=1; data[i]!='x' ;i++)//for all bytes not the end byte
    {
      power = ByteSize-2-i;//power of ten
      val = val+(data[i]-48)*pow(10,power);//converting the bytes from char to int
    } 
    LM_IN2_Val = val;
   }//end if 'W'
   
   if(data[0] == 'E')//RM_IN1 value
  {
   int power; //tracking the power of ten that any given int has
   for(int i=1; data[i]!='x' ;i++)//for all bytes not the end byte
    {
      power = ByteSize-2-i;//power of ten
      val = val+(data[i]-48)*pow(10,power);//converting the bytes from char to int
    } 
    RM_IN1_Val = val;
   }//end if 'E'

   if(data[0] == 'R')//RM_IN2 value
  {
   int power; //tracking the power of ten that any given int has
   for(int i=1; data[i]!='x' ;i++)//for all bytes not the end byte
    {
      power = ByteSize-2-i;//power of ten
      val = val+(data[i]-48)*pow(10,power);//converting the bytes from char to int
    } 
    RM_IN2_Val = val;
   }//end if 'R'   
   
   if(data[0] == 'O')// Open/Close servo angle
  {
   int power; //tracking the power of ten that any given int has
   for(int i=1; data[i]!='x' ;i++)//for all bytes not the end byte
    {
      power = ByteSize-2-i;//power of ten
      val = val+(data[i]-48)*pow(10,power);//converting the bytes from char to int
    } 
    Open_Ang = val;
   }//end if 'O'

   if(data[0] == 'U')// Raise/Lower servo angle
  {
   int power; //tracking the power of ten that any given int has
   for(int i=1; data[i]!='x' ;i++)//for all bytes not the end byte
    {
      power = ByteSize-2-i;//power of ten
      val = val+(data[i]-48)*pow(10,power);//converting the bytes from char to int
    } 
    Raise_Ang = val;
   }//end if 'U'      
  
   if(data[0] == 'L')// Launch State value
  {
   int power; //tracking the power of ten that any given int has
   for(int i=1; data[i]!='x' ;i++)//for all bytes not the end byte
    {
      power = ByteSize-2-i;//power of ten
      val = val+(data[i]-48)*pow(10,power);//converting the bytes from char to int
    } 
    Launch = val;
   }//end if 'L'   
   
 }//end if serial available
}//end com()

void launch()
{
  digitalWrite(Sol,Launch*255);
}

void LED()
{
  if(State == 1)// GREEN LED: Run state == GO
 {
   digitalWrite(GREEN_LED, HIGH);
   digitalWrite(RED_LED,LOW);
 }
  else //RED LED: Robot dead (Or possibly error): Run state == STOP
 {
   digitalWrite(GREEN_LED, LOW);
   digitalWrite(RED_LED,HIGH);
 } 
}//End LED()

void KILL()
{
  //Killstate motor values
    RM_IN1_Val = 0;
    RM_IN2_Val = 0;
    LM_IN1_Val = 0;
    LM_IN2_Val = 0;
  //Killstate servo angles
    Raise_Ang = 100;
    Open_Ang = 100;
  //Killstate Launch status
    Launch = 0;
}//END KILL()
 
void drive()
{
  //Left
  analogWrite(LM_IN1,LM_IN1_Val);
  analogWrite(LM_IN2,LM_IN2_Val);
  //Right
  analogWrite(RM_IN1,RM_IN1_Val);
  analogWrite(RM_IN2,RM_IN2_Val);
}//end drive()

void deBug()
{
  Serial.print("State = ");
  Serial.print(State);  
  Serial.print('\n');
  
  Serial.print("*** LEFT MOTOR ***");  
  Serial.print('\n');
  Serial.print("LM_IN1 = ");  
  Serial.print(LM_IN1_Val);
  Serial.print('\n');
  Serial.print("LM_IN2 = ");  
  Serial.print(LM_IN2_Val);
  Serial.print('\n');

  Serial.print("*** RIGHT MOTOR ***");  
  Serial.print('\n');
  Serial.print("RM_IN1 = ");  
  Serial.print(RM_IN1_Val);
  Serial.print('\n');
  Serial.print("RM_IN2 = ");  
  Serial.print(RM_IN2_Val);
  Serial.print('\n');

  Serial.print("*** SERVO ANGLES ***");  
  Serial.print('\n');
  Serial.print("Raise/Lower angle = ");  
  Serial.print(Raise_Ang);
  Serial.print('\n');
  Serial.print("Open/Close angle = ");  
  Serial.print(Open_Ang);
  Serial.print('\n');

  Serial.print("*** LAUNCH STATE ***");  
  Serial.print('\n');
  Serial.print("Launch = ");  
  Serial.print(Launch);
  Serial.print('\n');

  Serial.print('\n');  
}


void loop() //Main loop
{
  com();//listen for signals from controller
  if(Debug == 1)
  {
   deBug();
  }

  if(State==1)//Runstate == RUN (runswitch high)
  {
    //Enable run functions
    drive();
    servos();
    launch();
    LED();   
  }
  else//killswitch engaged
  {
    //Motors servos and launch solenoid set to stop values
    KILL();
    //Continually ping the motors servos and launch at the stop values
    drive();
    servos();
    launch();
    //indicate status
    LED();
  }// end kilswitch engaged
  
}//end main void() loop

[Dizzixx]

Well I figured out part of it.

I was printing a carriage return on the controllers serial line so I could view it in the serial monitor and forgot to comment it out before trying to communicate with the robot. I uploaded old code for the robot and I was able to control motors. Albeit not fantastically but it does work.

I then uploaded the new code to the robot and now it communicates for half a second and then I get a yellow and red flashing light on the servo controller and the robot stops responding entirely to commands from the controller.

[Dizzixx]

I got it working. Consider this closed. I found that the spinning the servo round and round was all that servo seems to want to do. I have it controlling a different servo just fine.

Still need help with the other servo though

HD-1440A Servo just spins and spins

So I solved the issue here. https://forum.pololu.com/t/micro-serial-servo-controller-xbee/3134/1 but have since decided to just use the arduinos servo library. Now I cant get the HD 1440A servo (pololu.com/catalog/product/1040) to hold a position. No matter how I send it pulses it just spins. I used the pololu micro servo controller in pololu mode, and the built in arduino servo library to set the angle in deg and to send it pulses of specific width using writeMicroseconds() and nothing works…