File content as of revision 0:67bc01af349e:

#include "mbed.h"
#include "QEI.h"
#include <cstdlib>
#include <ros.h>
#include <geometry_msgs/Vector3.h>
#include "Sabertooth.h"

Ticker flipper;
/*Serial pc(USBTX, USBRX);//Serial port*/
/*Serial xbee(p9, p10); //used to connected the mbed to the xbee over Serial UART comm.*/
//PwmOut wheel1(p21); //signal for speed/direction to right wheels
//PwmOut wheel2(p22); //singal for speed/direction to left wheels
QEI wheelR (p29, p30, NC, 624); // Signal from encoder in right wheel
QEI wheelL (p27, p28, NC, 624); // Signal from encoder in left wheel
DigitalOut led1(p20);  // LED for xbee connection verification
DigitalOut led2(p19);  // LED for W+ verification
DigitalOut led3(p18);  // LED for W- verification

float L=0.28; //Distance between left and right wheel
float R=0.03;//Radius of wheel
int encoderResolution=20000;// This is including the gear ratio
float vR = 0.00;
float vL = 0.00; //Due to the type of Pwm input the Sabertooth v1 takes, this is the signal for the robot to not move
float V=0;//Desired linear velocity of robot
float W=0;//Desired angular velocity of robot
int thetaprevR=0; // Prev angular rotation of motor
int thetacurrR=0; // Current angular rotation of motor
int thetaprevL=0; // Prev angular rotation of motor
int thetacurrL=0; // Current angular rotation of motor
float omegaR=0; // Angular velocity of the motor of right motor
float omegaL=0; // Angular velocity of the motor of left motor
float errR=0; // Error
float errL=0; // Error
float preverrR=0;//Error in the last time step
float interrR=0; //Integral of the error
float differrR=0;//Derivative of error
float preverrL=0;//Error in the last time step
float interrL=0; //Integral of the error
float differrL=0;//Derivative of error
float Kp=3.0;// Propotional controller constant
float Ki=0.001;//0.0000000001;//Integral controller constant
float Kd=0;//0.001;//Differential controller constant
float Kpw=0.1;// Propotional cotroller for omega
float Kiw=0.0001;// Propotional cotroller for omega
float errOmega=0;// Error in angular velocity
float errOmegaprev=0;// Error in angular velocity in last step
float interrOmega=0;// Integral of error in angular velocity
float dt=0.01; // Time step
float omegaDesiredR=((2*V-W*L)/(2*R*6.28)); // Desired angular velocity of right wheel
float omegaDesiredL=((2*V+W*L)/(2*R*6.28)); //Desired angular velocity of left wheel 
float thetaTotal=0;
char w[32];


 void checkVelocity(){
    
    thetacurrR=wheelR.getPulses();
    thetacurrL=wheelL.getPulses();
    omegaR=(float)(thetacurrR-thetaprevR)/(encoderResolution*dt);
    omegaL=(float)(thetacurrL-thetaprevL)/(encoderResolution*dt);
    thetaprevR=thetacurrR;
    thetaprevL=thetacurrL;
    errR=omegaDesiredR-omegaR;
    errL=omegaDesiredL-omegaL;
    interrR=interrR+dt*errR;
    interrL=interrL+dt*errL;
    differrR=(errR-preverrR)/dt;
    differrL=(errL-preverrL)/dt;
    errOmega=W-(((omegaL-omegaR)*2*3.14*R)/L);
    interrOmega=interrOmega+dt*errOmega;

   if((errL>0.01||errL<-0.01)){
            //vL+=(Kp*(errL)+Ki*interrL);//-Kpw*errOmega);//-Kiw*interrOmega);
            vL+=(Kp*(errL)+Kd*(differrL)+Ki*interrL);//-Kpw*errOmega);//-Kiw*interrOmega);
   }
   
   
   if((errR>0.01||errR<-0.01)){
            //vR+=(Kp*(errR)+Ki*interrR);//-Kpw*errOmega);//-Kiw*interrOmega);
            vR+=(Kp*(errR)+Kd*(differrR)+Ki*interrR);//-Kpw*errOmega);//-Kiw*interrOmega);
   } 
   
   if(interrR>20){
    interrR = 20;
}
if(interrL>20){
    interrL = 20;
}
if(interrR<-20){
    interrR = -20;
}
if(interrL<-20){
    interrL = -20;
}
    preverrR=errR;
    preverrL=errL;
    errOmegaprev=errOmega;
    
    
/* if(xbee.readable()){*/
        //xbee.scanf("%s\n",w);
        //pc.printf("%s   ",w);
        
        //if((atof(w)/1000.0)>=-0.7 && (atof(w)/1000.0)<0.7)
        //{W=atof(w)/1000.0;}
        
         omegaDesiredR=((2*V-W*L)/(2*R*6.28));
         omegaDesiredL=((2*V+W*L)/(2*R*6.28));  
/*}*/
 
} 
 
void cmd(const geometry_msgs::Vector3 &cmd_msg) {
     V = (float) cmd_msg.x;
     W = (float) cmd_msg.y;  
     led3 = 1;
}
 
ros::NodeHandle nh(p9,p10);
ros::Subscriber<geometry_msgs::Vector3> sub("cmd_vel_w", cmd);
Sabertooth sb(p13,129,9600);


int main() {
    
    nh.initNode();
    nh.subscribe(sub);
    sb.InitializeCom();

    //wheel1.period(0.20);
    //wheel2.period(0.20);
      
    flipper.attach(&checkVelocity, dt);
    while(1){
        nh.spinOnce();
    wait_ms(1);
    if (V>0)
    {
    led1 = 1;
    }
    
        sb.SetSpeedMotorA((int)vR);
    sb.SetSpeedMotorB((int)vL);
    
        //wheel1.pulsewidth(vR); //output the current speed value to right motor
        //wheel2.pulsewidth(vL); //output the current speed value to left motor  
/*        wheel1.pulsewidth(0.00155);*/
        //wheel2.pulsewidth(0.001);
       
    }
}