USNA WSE ES456 / Mbed 2 deprecated Madpulse_Speed_Control_Students

Push for Students

Dependencies:   BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed

Fork of Madpulse_Speed_Control_temp by USNA WSE ES456

madpulse_ctrl.cpp@0:daea75c21ac1, 2017-11-14 (annotated)

Committer:
jdawkins
Date:
Tue Nov 14 13:51:29 2017 +0000
Revision:
0:daea75c21ac1
Child:
1:f4c9926fb4c9
Initial commit of Madpulse Control Instructors Solution

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 0:daea75c21ac1 1 #include "madpulse.h"
jdawkins 0:daea75c21ac1 2
jdawkins 0:daea75c21ac1 3 #define BUFFER_SIZE 1024
jdawkins 0:daea75c21ac1 4
jdawkins 0:daea75c21ac1 5 #define FIVE_HZ_LOOP 5.0
jdawkins 0:daea75c21ac1 6 #define LOG_LOOP_FREQ 25.0 // Four messages are taking turns logging, it should be about 25Hz overall
jdawkins 0:daea75c21ac1 7 #define CTRL_LOOP_FREQ 100.0
jdawkins 0:daea75c21ac1 8 #define MAIN_LOOP_FREQ 300.0
jdawkins 0:daea75c21ac1 9 #define LOG_IMU 1
jdawkins 0:daea75c21ac1 10 #define LOG_MAG 2
jdawkins 0:daea75c21ac1 11 #define LOG_EUL 3
jdawkins 0:daea75c21ac1 12 #define LOG_ODO 4
jdawkins 0:daea75c21ac1 13 #define VEH_ID 1
jdawkins 0:daea75c21ac1 14
jdawkins 0:daea75c21ac1 15 #define PI (3.14159)
jdawkins 0:daea75c21ac1 16
jdawkins 0:daea75c21ac1 17
jdawkins 0:daea75c21ac1 18 // Instantiate Serial Ports
jdawkins 0:daea75c21ac1 19 Serial pc(USBTX, USBRX, 115200); // USB UART Terminal
jdawkins 0:daea75c21ac1 20 Serial xbee(p13,p14,115200);
jdawkins 0:daea75c21ac1 21
jdawkins 0:daea75c21ac1 22 // Instantiate Class Objects
jdawkins 0:daea75c21ac1 23 BNO055 imu(p9,p10); // BNO055 Class Object I2C(SDL,SCL)
jdawkins 0:daea75c21ac1 24 ServoIn CH1(p15); // Read RC Servo Channel 1 (steering) Pin 15
jdawkins 0:daea75c21ac1 25 ServoIn CH2(p16); // Read RC Servo Channel 2 (Throttle) Pin 16
jdawkins 0:daea75c21ac1 26 InterruptIn wheel_irq(p17); // Attach Interrupt for Wheel Encoder Pin 17
jdawkins 0:daea75c21ac1 27 ServoOut Steer(p21); // Steer command object on Pin 21 goes to steering servo
jdawkins 0:daea75c21ac1 28 ServoOut Throttle(p22); // Throttle command object on Pin 22 goes to esc
jdawkins 0:daea75c21ac1 29
jdawkins 0:daea75c21ac1 30 Ticker ctrl_ticker;
jdawkins 0:daea75c21ac1 31
jdawkins 0:daea75c21ac1 32 // Create Instance of Madpulse class which manages low level vehicle functions and exposes a set of methods for
jdawkins 0:daea75c21ac1 33 // controlling the vehicle
jdawkins 0:daea75c21ac1 34 MadPulse mp(VEH_ID,&xbee,&imu,&CH1,&CH2,&wheel_irq,&Steer,&Throttle);
jdawkins 0:daea75c21ac1 35
jdawkins 0:daea75c21ac1 36 // Control Gains
jdawkins 0:daea75c21ac1 37
jdawkins 0:daea75c21ac1 38 float Kp_spd = 0.06;
jdawkins 0:daea75c21ac1 39 float Ki_spd = 0.05;
jdawkins 0:daea75c21ac1 40
jdawkins 0:daea75c21ac1 41 float Kp_psi = 0.65;
jdawkins 0:daea75c21ac1 42 float Kd_psi = 0.1;
jdawkins 0:daea75c21ac1 43 float L = 0.01;
jdawkins 0:daea75c21ac1 44
jdawkins 0:daea75c21ac1 45 float psi_est,psi_est_old;
jdawkins 0:daea75c21ac1 46 float spd_err,spd_err_i;
jdawkins 0:daea75c21ac1 47 float spd,des_spd;
jdawkins 0:daea75c21ac1 48 float dist;
jdawkins 0:daea75c21ac1 49
jdawkins 0:daea75c21ac1 50 Timer t; // Create Timer
jdawkins 0:daea75c21ac1 51 int log_it; // Iterator for Spacing logging of different messages
jdawkins 0:daea75c21ac1 52
jdawkins 0:daea75c21ac1 53 uint8_t buf[BUFFER_SIZE];
jdawkins 0:daea75c21ac1 54
jdawkins 0:daea75c21ac1 55 float t_start,t_log,t_ctrl,t_5hz,t_run; // Create timer variables
jdawkins 0:daea75c21ac1 56 float dt;
jdawkins 0:daea75c21ac1 57 float str,thr,wheel_spd;
jdawkins 0:daea75c21ac1 58 float wrapTo2pi(float ang);
jdawkins 0:daea75c21ac1 59 float wrapTopi(float ang);
jdawkins 0:daea75c21ac1 60 bool rc_conn,auto_flag;
jdawkins 0:daea75c21ac1 61
jdawkins 0:daea75c21ac1 62 void initializeControl(){
jdawkins 0:daea75c21ac1 63 printf("Initialize Control\r\n");
jdawkins 0:daea75c21ac1 64 t_run = t.read();
jdawkins 0:daea75c21ac1 65 }
jdawkins 0:daea75c21ac1 66
jdawkins 0:daea75c21ac1 67 void stopControl(){
jdawkins 0:daea75c21ac1 68 printf("Stop Control\r\n");
jdawkins 0:daea75c21ac1 69 }
jdawkins 0:daea75c21ac1 70 void readRCSignal(){
jdawkins 0:daea75c21ac1 71 if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
jdawkins 0:daea75c21ac1 72 rc_conn = false;
jdawkins 0:daea75c21ac1 73 } else {
jdawkins 0:daea75c21ac1 74 rc_conn = true;
jdawkins 0:daea75c21ac1 75 } // end if(Channels connected)
jdawkins 0:daea75c21ac1 76
jdawkins 0:daea75c21ac1 77 if( (abs(CH1.servoPulse-1500) > 200) || (abs(CH2.servoPulse-1500) > 200)){
jdawkins 0:daea75c21ac1 78 auto_flag = 0;
jdawkins 0:daea75c21ac1 79 }
jdawkins 0:daea75c21ac1 80 }
jdawkins 0:daea75c21ac1 81 void readKeyboard(){
jdawkins 0:daea75c21ac1 82 if(xbee.readable()) {
jdawkins 0:daea75c21ac1 83 char c = xbee.getc();
jdawkins 0:daea75c21ac1 84 if(c == 'H' || c == 'h') {
jdawkins 0:daea75c21ac1 85 xbee.printf("Command List...\r\n");
jdawkins 0:daea75c21ac1 86 xbee.printf("d - set open loop duty cycle\r\n");
jdawkins 0:daea75c21ac1 87 xbee.printf("r - run open loop control with current duty cycle\r\n");
jdawkins 0:daea75c21ac1 88 xbee.printf("h - display this prompt and exit\r\n");
jdawkins 0:daea75c21ac1 89 xbee.printf("t - set duration of test\r\n\n\n");
jdawkins 0:daea75c21ac1 90 xbee.printf("Enter Command\r\n");
jdawkins 0:daea75c21ac1 91 }
jdawkins 0:daea75c21ac1 92
jdawkins 0:daea75c21ac1 93 if(c == 'R' || c == 'r') {
jdawkins 0:daea75c21ac1 94 auto_flag = true;
jdawkins 0:daea75c21ac1 95 initializeControl();
jdawkins 0:daea75c21ac1 96 }
jdawkins 0:daea75c21ac1 97 if(c == 'S' || c == 's') {
jdawkins 0:daea75c21ac1 98 auto_flag = false;
jdawkins 0:daea75c21ac1 99 }
jdawkins 0:daea75c21ac1 100
jdawkins 0:daea75c21ac1 101 if(c == 'T' || c == 't') {
jdawkins 0:daea75c21ac1 102
jdawkins 0:daea75c21ac1 103 }
jdawkins 0:daea75c21ac1 104 }
jdawkins 0:daea75c21ac1 105 }
jdawkins 0:daea75c21ac1 106 void controlLoop(){
jdawkins 0:daea75c21ac1 107 dt = t.read()-t_ctrl;
jdawkins 0:daea75c21ac1 108
jdawkins 0:daea75c21ac1 109 if(auto_flag){ // If vehicle is in Auto Mode
jdawkins 0:daea75c21ac1 110
jdawkins 0:daea75c21ac1 111 float des_psi = 0;
jdawkins 0:daea75c21ac1 112 spd = mp.getFreq()/27.8119;
jdawkins 0:daea75c21ac1 113 //str = -Kp_psi*wrapTopi(des_psi-wrapTopi(mp.euler.yaw));
jdawkins 0:daea75c21ac1 114 str = 0.001;
jdawkins 0:daea75c21ac1 115 if(t.read()-t_run < 10){
jdawkins 0:daea75c21ac1 116 des_spd = 1.5;
jdawkins 0:daea75c21ac1 117 spd_err = des_spd-spd;
jdawkins 0:daea75c21ac1 118 spd_err_i += spd_err*dt;
jdawkins 0:daea75c21ac1 119 // spd_err_d = (spd - spd_old)/dt;
jdawkins 0:daea75c21ac1 120
jdawkins 0:daea75c21ac1 121
jdawkins 0:daea75c21ac1 122 thr = Kp_spd*spd_err + Ki_spd*spd_err_i + 0.04;
jdawkins 0:daea75c21ac1 123
jdawkins 0:daea75c21ac1 124 // thr = 0.04;
jdawkins 0:daea75c21ac1 125 }else{
jdawkins 0:daea75c21ac1 126 thr = 0.0;
jdawkins 0:daea75c21ac1 127 }
jdawkins 0:daea75c21ac1 128 }else{
jdawkins 0:daea75c21ac1 129 str = ((CH1.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:daea75c21ac1 130 thr = ((CH2.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:daea75c21ac1 131 }
jdawkins 0:daea75c21ac1 132
jdawkins 0:daea75c21ac1 133 t_ctrl = t.read();
jdawkins 0:daea75c21ac1 134 }
jdawkins 0:daea75c21ac1 135
jdawkins 0:daea75c21ac1 136
jdawkins 0:daea75c21ac1 137 int main()
jdawkins 0:daea75c21ac1 138 {
jdawkins 0:daea75c21ac1 139
jdawkins 0:daea75c21ac1 140 log_it = 0;
jdawkins 0:daea75c21ac1 141 auto_mode = 0;
jdawkins 0:daea75c21ac1 142 // Default Steering and Throttle commands to neutral
jdawkins 0:daea75c21ac1 143 Steer.write(1500); // Write Steering Pulse
jdawkins 0:daea75c21ac1 144 Throttle.write(1500);
jdawkins 0:daea75c21ac1 145
jdawkins 0:daea75c21ac1 146
jdawkins 0:daea75c21ac1 147 t.start();
jdawkins 0:daea75c21ac1 148 t_start = t.read();
jdawkins 0:daea75c21ac1 149 t_log = t.read();
jdawkins 0:daea75c21ac1 150 t_ctrl = t.read();
jdawkins 0:daea75c21ac1 151 t_5hz = t.read();
jdawkins 0:daea75c21ac1 152
jdawkins 0:daea75c21ac1 153 while(1) {
jdawkins 0:daea75c21ac1 154 auto_LED = auto_flag;
jdawkins 0:daea75c21ac1 155 dwm_LED = rc_conn;
jdawkins 0:daea75c21ac1 156 readKeyboard(); // Check for Input from Keyboard over xbee
jdawkins 0:daea75c21ac1 157
jdawkins 0:daea75c21ac1 158 // check for servo pulse from either channel of receiver module
jdawkins 0:daea75c21ac1 159 readRCSignal();
jdawkins 0:daea75c21ac1 160
jdawkins 0:daea75c21ac1 161
jdawkins 0:daea75c21ac1 162
jdawkins 0:daea75c21ac1 163
jdawkins 0:daea75c21ac1 164
jdawkins 0:daea75c21ac1 165 if(t.read()-t_5hz > (1.0/FIVE_HZ_LOOP)) {
jdawkins 0:daea75c21ac1 166 // 5 Hz loop intended for debugging print statements
jdawkins 0:daea75c21ac1 167
jdawkins 0:daea75c21ac1 168 // xbee.printf("Auto Cond: %f , %f \r\n",abs(CH1.servoPulse-1500),abs(CH2.servoPulse-1500));
jdawkins 0:daea75c21ac1 169 t_5hz = t.read();
jdawkins 0:daea75c21ac1 170
jdawkins 0:daea75c21ac1 171 }
jdawkins 0:daea75c21ac1 172
jdawkins 0:daea75c21ac1 173 if(t.read()-t_ctrl > (1.0/CTRL_LOOP_FREQ)) {
jdawkins 0:daea75c21ac1 174 // 100 Hz loop for calling control loop
jdawkins 0:daea75c21ac1 175 controlLoop();
jdawkins 0:daea75c21ac1 176 t_ctrl = t.read();
jdawkins 0:daea75c21ac1 177 }
jdawkins 0:daea75c21ac1 178
jdawkins 0:daea75c21ac1 179
jdawkins 0:daea75c21ac1 180
jdawkins 0:daea75c21ac1 181 if(t.read()-t_log > (1.0/LOG_LOOP_FREQ)) {
jdawkins 0:daea75c21ac1 182 // Faster loop for logging data for plotting and analysis
jdawkins 0:daea75c21ac1 183
jdawkins 0:daea75c21ac1 184 mp.getAccel();
jdawkins 0:daea75c21ac1 185 mp.getGyro();
jdawkins 0:daea75c21ac1 186 // xbee.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", mp.accel.x,mp.accel.y,mp.accel.z,mp.gyro.x,mp.gyro.y,mp.gyro.z);
jdawkins 0:daea75c21ac1 187
jdawkins 0:daea75c21ac1 188
jdawkins 0:daea75c21ac1 189 mp.getMag();
jdawkins 0:daea75c21ac1 190 //xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",mp.mag.x,mp.mag.y,mp.mag.z);
jdawkins 0:daea75c21ac1 191
jdawkins 0:daea75c21ac1 192
jdawkins 0:daea75c21ac1 193 mp.getEuler();
jdawkins 0:daea75c21ac1 194 // xbee.printf("$RPY,%.3f, %.3f, %.3f\r\n", mp.euler.roll,mp.euler.pitch,wrapTo2pi(mp.euler.yaw));
jdawkins 0:daea75c21ac1 195
jdawkins 0:daea75c21ac1 196
jdawkins 0:daea75c21ac1 197 mp.getSpeed();
jdawkins 0:daea75c21ac1 198 xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);
jdawkins 0:daea75c21ac1 199
jdawkins 0:daea75c21ac1 200
jdawkins 0:daea75c21ac1 201
jdawkins 0:daea75c21ac1 202 t_log = t.read();
jdawkins 0:daea75c21ac1 203 }
jdawkins 0:daea75c21ac1 204
jdawkins 0:daea75c21ac1 205 if(rc_conn) {
jdawkins 0:daea75c21ac1 206 // str = ((CH1.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:daea75c21ac1 207 // thr = ((CH2.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:daea75c21ac1 208 mp.setSteer(str);
jdawkins 0:daea75c21ac1 209 mp.setThrottle(thr);
jdawkins 0:daea75c21ac1 210
jdawkins 0:daea75c21ac1 211 } else {
jdawkins 0:daea75c21ac1 212 Steer.write(1500); // Write Steering Pulse
jdawkins 0:daea75c21ac1 213 Throttle.write(1500);
jdawkins 0:daea75c21ac1 214 // mp.setSteer(str);
jdawkins 0:daea75c21ac1 215 // mp.setThrottle(thr);
jdawkins 0:daea75c21ac1 216 }
jdawkins 0:daea75c21ac1 217
jdawkins 0:daea75c21ac1 218 wait(1.0/MAIN_LOOP_FREQ);
jdawkins 0:daea75c21ac1 219 } // End While 1 Loop
jdawkins 0:daea75c21ac1 220 } // End Main Function
jdawkins 0:daea75c21ac1 221
jdawkins 0:daea75c21ac1 222 float wrapTo2pi(float ang)
jdawkins 0:daea75c21ac1 223 {
jdawkins 0:daea75c21ac1 224 if(ang > 2*PI) {
jdawkins 0:daea75c21ac1 225 ang = ang - 2*PI;
jdawkins 0:daea75c21ac1 226 }
jdawkins 0:daea75c21ac1 227
jdawkins 0:daea75c21ac1 228 if(ang < 0) {
jdawkins 0:daea75c21ac1 229 ang = ang + 2*PI;
jdawkins 0:daea75c21ac1 230 }
jdawkins 0:daea75c21ac1 231 return ang;
jdawkins 0:daea75c21ac1 232 }
jdawkins 0:daea75c21ac1 233
jdawkins 0:daea75c21ac1 234 float wrapTopi(float ang)
jdawkins 0:daea75c21ac1 235 {
jdawkins 0:daea75c21ac1 236 if(ang > PI) {
jdawkins 0:daea75c21ac1 237 ang = ang - 2*PI;
jdawkins 0:daea75c21ac1 238 }
jdawkins 0:daea75c21ac1 239
jdawkins 0:daea75c21ac1 240 if(ang < -PI) {
jdawkins 0:daea75c21ac1 241 ang = ang + 2*PI;
jdawkins 0:daea75c21ac1 242 }
jdawkins 0:daea75c21ac1 243 return ang;
jdawkins 0:daea75c21ac1 244 }
jdawkins 0:daea75c21ac1 245