![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
controller for 2 motor robot with ROS
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?
User | Revision | Line number | New 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 | } |