Mitchell Jones / Mbed 2 deprecated ROSRobot2

controller for 2 motor robot with ROS

Dependencies:   mbed

main.cpp@0:67bc01af349e, 2016-04-26 (annotated)

Committer:
mitchelljones
Date:
Tue Apr 26 21:36:34 2016 +0000
Revision:
0:67bc01af349e
Two-motor ground robot controller with ROS coms.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mitchelljones 0:67bc01af349e 1 #include "mbed.h"
mitchelljones 0:67bc01af349e 2 #include "QEI.h"
mitchelljones 0:67bc01af349e 3 #include <cstdlib>
mitchelljones 0:67bc01af349e 4 #include <ros.h>
mitchelljones 0:67bc01af349e 5 #include <geometry_msgs/Vector3.h>
mitchelljones 0:67bc01af349e 6 #include "Sabertooth.h"
mitchelljones 0:67bc01af349e 7
mitchelljones 0:67bc01af349e 8 Ticker flipper;
mitchelljones 0:67bc01af349e 9 /*Serial pc(USBTX, USBRX);//Serial port*/
mitchelljones 0:67bc01af349e 10 /*Serial xbee(p9, p10); //used to connected the mbed to the xbee over Serial UART comm.*/
mitchelljones 0:67bc01af349e 11 //PwmOut wheel1(p21); //signal for speed/direction to right wheels
mitchelljones 0:67bc01af349e 12 //PwmOut wheel2(p22); //singal for speed/direction to left wheels
mitchelljones 0:67bc01af349e 13 QEI wheelR (p29, p30, NC, 624); // Signal from encoder in right wheel
mitchelljones 0:67bc01af349e 14 QEI wheelL (p27, p28, NC, 624); // Signal from encoder in left wheel
mitchelljones 0:67bc01af349e 15 DigitalOut led1(p20); // LED for xbee connection verification
mitchelljones 0:67bc01af349e 16 DigitalOut led2(p19); // LED for W+ verification
mitchelljones 0:67bc01af349e 17 DigitalOut led3(p18); // LED for W- verification
mitchelljones 0:67bc01af349e 18
mitchelljones 0:67bc01af349e 19 float L=0.28; //Distance between left and right wheel
mitchelljones 0:67bc01af349e 20 float R=0.03;//Radius of wheel
mitchelljones 0:67bc01af349e 21 int encoderResolution=20000;// This is including the gear ratio
mitchelljones 0:67bc01af349e 22 float vR = 0.00;
mitchelljones 0:67bc01af349e 23 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
mitchelljones 0:67bc01af349e 24 float V=0;//Desired linear velocity of robot
mitchelljones 0:67bc01af349e 25 float W=0;//Desired angular velocity of robot
mitchelljones 0:67bc01af349e 26 int thetaprevR=0; // Prev angular rotation of motor
mitchelljones 0:67bc01af349e 27 int thetacurrR=0; // Current angular rotation of motor
mitchelljones 0:67bc01af349e 28 int thetaprevL=0; // Prev angular rotation of motor
mitchelljones 0:67bc01af349e 29 int thetacurrL=0; // Current angular rotation of motor
mitchelljones 0:67bc01af349e 30 float omegaR=0; // Angular velocity of the motor of right motor
mitchelljones 0:67bc01af349e 31 float omegaL=0; // Angular velocity of the motor of left motor
mitchelljones 0:67bc01af349e 32 float errR=0; // Error
mitchelljones 0:67bc01af349e 33 float errL=0; // Error
mitchelljones 0:67bc01af349e 34 float preverrR=0;//Error in the last time step
mitchelljones 0:67bc01af349e 35 float interrR=0; //Integral of the error
mitchelljones 0:67bc01af349e 36 float differrR=0;//Derivative of error
mitchelljones 0:67bc01af349e 37 float preverrL=0;//Error in the last time step
mitchelljones 0:67bc01af349e 38 float interrL=0; //Integral of the error
mitchelljones 0:67bc01af349e 39 float differrL=0;//Derivative of error
mitchelljones 0:67bc01af349e 40 float Kp=3.0;// Propotional controller constant
mitchelljones 0:67bc01af349e 41 float Ki=0.001;//0.0000000001;//Integral controller constant
mitchelljones 0:67bc01af349e 42 float Kd=0;//0.001;//Differential controller constant
mitchelljones 0:67bc01af349e 43 float Kpw=0.1;// Propotional cotroller for omega
mitchelljones 0:67bc01af349e 44 float Kiw=0.0001;// Propotional cotroller for omega
mitchelljones 0:67bc01af349e 45 float errOmega=0;// Error in angular velocity
mitchelljones 0:67bc01af349e 46 float errOmegaprev=0;// Error in angular velocity in last step
mitchelljones 0:67bc01af349e 47 float interrOmega=0;// Integral of error in angular velocity
mitchelljones 0:67bc01af349e 48 float dt=0.01; // Time step
mitchelljones 0:67bc01af349e 49 float omegaDesiredR=((2*V-W*L)/(2*R*6.28)); // Desired angular velocity of right wheel
mitchelljones 0:67bc01af349e 50 float omegaDesiredL=((2*V+W*L)/(2*R*6.28)); //Desired angular velocity of left wheel
mitchelljones 0:67bc01af349e 51 float thetaTotal=0;
mitchelljones 0:67bc01af349e 52 char w[32];
mitchelljones 0:67bc01af349e 53
mitchelljones 0:67bc01af349e 54
mitchelljones 0:67bc01af349e 55 void checkVelocity(){
mitchelljones 0:67bc01af349e 56
mitchelljones 0:67bc01af349e 57 thetacurrR=wheelR.getPulses();
mitchelljones 0:67bc01af349e 58 thetacurrL=wheelL.getPulses();
mitchelljones 0:67bc01af349e 59 omegaR=(float)(thetacurrR-thetaprevR)/(encoderResolution*dt);
mitchelljones 0:67bc01af349e 60 omegaL=(float)(thetacurrL-thetaprevL)/(encoderResolution*dt);
mitchelljones 0:67bc01af349e 61 thetaprevR=thetacurrR;
mitchelljones 0:67bc01af349e 62 thetaprevL=thetacurrL;
mitchelljones 0:67bc01af349e 63 errR=omegaDesiredR-omegaR;
mitchelljones 0:67bc01af349e 64 errL=omegaDesiredL-omegaL;
mitchelljones 0:67bc01af349e 65 interrR=interrR+dt*errR;
mitchelljones 0:67bc01af349e 66 interrL=interrL+dt*errL;
mitchelljones 0:67bc01af349e 67 differrR=(errR-preverrR)/dt;
mitchelljones 0:67bc01af349e 68 differrL=(errL-preverrL)/dt;
mitchelljones 0:67bc01af349e 69 errOmega=W-(((omegaL-omegaR)*2*3.14*R)/L);
mitchelljones 0:67bc01af349e 70 interrOmega=interrOmega+dt*errOmega;
mitchelljones 0:67bc01af349e 71
mitchelljones 0:67bc01af349e 72 if((errL>0.01||errL<-0.01)){
mitchelljones 0:67bc01af349e 73 //vL+=(Kp*(errL)+Ki*interrL);//-Kpw*errOmega);//-Kiw*interrOmega);
mitchelljones 0:67bc01af349e 74 vL+=(Kp*(errL)+Kd*(differrL)+Ki*interrL);//-Kpw*errOmega);//-Kiw*interrOmega);
mitchelljones 0:67bc01af349e 75 }
mitchelljones 0:67bc01af349e 76
mitchelljones 0:67bc01af349e 77
mitchelljones 0:67bc01af349e 78 if((errR>0.01||errR<-0.01)){
mitchelljones 0:67bc01af349e 79 //vR+=(Kp*(errR)+Ki*interrR);//-Kpw*errOmega);//-Kiw*interrOmega);
mitchelljones 0:67bc01af349e 80 vR+=(Kp*(errR)+Kd*(differrR)+Ki*interrR);//-Kpw*errOmega);//-Kiw*interrOmega);
mitchelljones 0:67bc01af349e 81 }
mitchelljones 0:67bc01af349e 82
mitchelljones 0:67bc01af349e 83 if(interrR>20){
mitchelljones 0:67bc01af349e 84 interrR = 20;
mitchelljones 0:67bc01af349e 85 }
mitchelljones 0:67bc01af349e 86 if(interrL>20){
mitchelljones 0:67bc01af349e 87 interrL = 20;
mitchelljones 0:67bc01af349e 88 }
mitchelljones 0:67bc01af349e 89 if(interrR<-20){
mitchelljones 0:67bc01af349e 90 interrR = -20;
mitchelljones 0:67bc01af349e 91 }
mitchelljones 0:67bc01af349e 92 if(interrL<-20){
mitchelljones 0:67bc01af349e 93 interrL = -20;
mitchelljones 0:67bc01af349e 94 }
mitchelljones 0:67bc01af349e 95 preverrR=errR;
mitchelljones 0:67bc01af349e 96 preverrL=errL;
mitchelljones 0:67bc01af349e 97 errOmegaprev=errOmega;
mitchelljones 0:67bc01af349e 98
mitchelljones 0:67bc01af349e 99
mitchelljones 0:67bc01af349e 100 /* if(xbee.readable()){*/
mitchelljones 0:67bc01af349e 101 //xbee.scanf("%s\n",w);
mitchelljones 0:67bc01af349e 102 //pc.printf("%s ",w);
mitchelljones 0:67bc01af349e 103
mitchelljones 0:67bc01af349e 104 //if((atof(w)/1000.0)>=-0.7 && (atof(w)/1000.0)<0.7)
mitchelljones 0:67bc01af349e 105 //{W=atof(w)/1000.0;}
mitchelljones 0:67bc01af349e 106
mitchelljones 0:67bc01af349e 107 omegaDesiredR=((2*V-W*L)/(2*R*6.28));
mitchelljones 0:67bc01af349e 108 omegaDesiredL=((2*V+W*L)/(2*R*6.28));
mitchelljones 0:67bc01af349e 109 /*}*/
mitchelljones 0:67bc01af349e 110
mitchelljones 0:67bc01af349e 111 }
mitchelljones 0:67bc01af349e 112
mitchelljones 0:67bc01af349e 113 void cmd(const geometry_msgs::Vector3 &cmd_msg) {
mitchelljones 0:67bc01af349e 114 V = (float) cmd_msg.x;
mitchelljones 0:67bc01af349e 115 W = (float) cmd_msg.y;
mitchelljones 0:67bc01af349e 116 led3 = 1;
mitchelljones 0:67bc01af349e 117 }
mitchelljones 0:67bc01af349e 118
mitchelljones 0:67bc01af349e 119 ros::NodeHandle nh(p9,p10);
mitchelljones 0:67bc01af349e 120 ros::Subscriber<geometry_msgs::Vector3> sub("cmd_vel_w", cmd);
mitchelljones 0:67bc01af349e 121 Sabertooth sb(p13,129,9600);
mitchelljones 0:67bc01af349e 122
mitchelljones 0:67bc01af349e 123
mitchelljones 0:67bc01af349e 124 int main() {
mitchelljones 0:67bc01af349e 125
mitchelljones 0:67bc01af349e 126 nh.initNode();
mitchelljones 0:67bc01af349e 127 nh.subscribe(sub);
mitchelljones 0:67bc01af349e 128 sb.InitializeCom();
mitchelljones 0:67bc01af349e 129
mitchelljones 0:67bc01af349e 130 //wheel1.period(0.20);
mitchelljones 0:67bc01af349e 131 //wheel2.period(0.20);
mitchelljones 0:67bc01af349e 132
mitchelljones 0:67bc01af349e 133 flipper.attach(&checkVelocity, dt);
mitchelljones 0:67bc01af349e 134 while(1){
mitchelljones 0:67bc01af349e 135 nh.spinOnce();
mitchelljones 0:67bc01af349e 136 wait_ms(1);
mitchelljones 0:67bc01af349e 137 if (V>0)
mitchelljones 0:67bc01af349e 138 {
mitchelljones 0:67bc01af349e 139 led1 = 1;
mitchelljones 0:67bc01af349e 140 }
mitchelljones 0:67bc01af349e 141
mitchelljones 0:67bc01af349e 142 sb.SetSpeedMotorA((int)vR);
mitchelljones 0:67bc01af349e 143 sb.SetSpeedMotorB((int)vL);
mitchelljones 0:67bc01af349e 144
mitchelljones 0:67bc01af349e 145 //wheel1.pulsewidth(vR); //output the current speed value to right motor
mitchelljones 0:67bc01af349e 146 //wheel2.pulsewidth(vL); //output the current speed value to left motor
mitchelljones 0:67bc01af349e 147 /* wheel1.pulsewidth(0.00155);*/
mitchelljones 0:67bc01af349e 148 //wheel2.pulsewidth(0.001);
mitchelljones 0:67bc01af349e 149
mitchelljones 0:67bc01af349e 150 }
mitchelljones 0:67bc01af349e 151 }