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@2:2bb375ab3b2b, 2017-11-15 (annotated)

Committer:
jdawkins
Date:
Wed Nov 15 21:51:31 2017 +0000
Revision:
2:2bb375ab3b2b
Parent:
1:f4c9926fb4c9 
Initial Template for Project Part 1

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 2:2bb375ab3b2b 36 // Control Gains
jdawkins 0:daea75c21ac1 37
jdawkins 2:2bb375ab3b2b 38 float Kp_spd = 0.0;
jdawkins 2:2bb375ab3b2b 39 float Ki_spd = 0.0;
jdawkins 2:2bb375ab3b2b 40
jdawkins 2:2bb375ab3b2b 41 float Kp_psi = 0.0;
jdawkins 2:2bb375ab3b2b 42 float Kd_psi = 0.0;
jdawkins 2:2bb375ab3b2b 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 1:f4c9926fb4c9 60 bool rc_conn,auto_flag,log_flag;
jdawkins 1:f4c9926fb4c9 61
jdawkins 1:f4c9926fb4c9 62
jdawkins 2:2bb375ab3b2b 63 // ============================================
jdawkins 2:2bb375ab3b2b 64 // Control Loop
jdawkins 2:2bb375ab3b2b 65 // ============================================
jdawkins 2:2bb375ab3b2b 66 // Heading & Speed Control Function
jdawkins 2:2bb375ab3b2b 67 // Sampled at 100 HZ
jdawkins 1:f4c9926fb4c9 68 void controlLoop(){
jdawkins 2:2bb375ab3b2b 69 dt = t.read()-t_ctrl; // Sample time (sec)
jdawkins 1:f4c9926fb4c9 70
jdawkins 2:2bb375ab3b2b 71 if(auto_flag){
jdawkins 2:2bb375ab3b2b 72 // Vehicle is in Auto Mode
jdawkins 1:f4c9926fb4c9 73
jdawkins 2:2bb375ab3b2b 74 spd = mp.getFreq()*0.0436; // Read odmeter (meters/sec)
jdawkins 2:2bb375ab3b2b 75
jdawkins 2:2bb375ab3b2b 76 // Steering Command (+/- 1)
jdawkins 2:2bb375ab3b2b 77 str = 0.0; // Set steering trim
jdawkins 2:2bb375ab3b2b 78
jdawkins 2:2bb375ab3b2b 79 // Throttle Command (+/- 1)
jdawkins 2:2bb375ab3b2b 80 if(t.read()-t_run < 1){ // Set throttle value for 1 secs
jdawkins 2:2bb375ab3b2b 81 thr = 0.05;
jdawkins 1:f4c9926fb4c9 82 }else{
jdawkins 1:f4c9926fb4c9 83 thr = 0.0;
jdawkins 1:f4c9926fb4c9 84 }
jdawkins 2:2bb375ab3b2b 85 // Use manual steering for speed control testing
jdawkins 2:2bb375ab3b2b 86 str = ((CH1.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 1:f4c9926fb4c9 87 }else{
jdawkins 2:2bb375ab3b2b 88 // Maunal RC Mode
jdawkins 1:f4c9926fb4c9 89 str = ((CH1.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 1:f4c9926fb4c9 90 thr = ((CH2.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 1:f4c9926fb4c9 91 }
jdawkins 1:f4c9926fb4c9 92
jdawkins 1:f4c9926fb4c9 93 t_ctrl = t.read();
jdawkins 1:f4c9926fb4c9 94 }
jdawkins 1:f4c9926fb4c9 95
jdawkins 2:2bb375ab3b2b 96 // ============================================
jdawkins 2:2bb375ab3b2b 97 // Data Log
jdawkins 2:2bb375ab3b2b 98 // ============================================
jdawkins 2:2bb375ab3b2b 99 // Print data to Xbee serial port (115200 Baud)
jdawkins 2:2bb375ab3b2b 100 void dataLog(){
jdawkins 2:2bb375ab3b2b 101 xbee.printf("%.3f,%d,%.3f,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),dist,spd,thr,str);
jdawkins 1:f4c9926fb4c9 102 }
jdawkins 1:f4c9926fb4c9 103
jdawkins 0:daea75c21ac1 104
jdawkins 0:daea75c21ac1 105 void initializeControl(){
jdawkins 0:daea75c21ac1 106 printf("Initialize Control\r\n");
jdawkins 0:daea75c21ac1 107 t_run = t.read();
jdawkins 2:2bb375ab3b2b 108 dist = 0.0;
jdawkins 2:2bb375ab3b2b 109 spd_err_i = 0;
jdawkins 0:daea75c21ac1 110 }
jdawkins 0:daea75c21ac1 111
jdawkins 0:daea75c21ac1 112 void stopControl(){
jdawkins 2:2bb375ab3b2b 113 printf("Stop Control\r\n");
jdawkins 2:2bb375ab3b2b 114 spd_err_i = 0;
jdawkins 0:daea75c21ac1 115 }
jdawkins 0:daea75c21ac1 116 void readRCSignal(){
jdawkins 0:daea75c21ac1 117 if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
jdawkins 0:daea75c21ac1 118 rc_conn = false;
jdawkins 0:daea75c21ac1 119 } else {
jdawkins 0:daea75c21ac1 120 rc_conn = true;
jdawkins 0:daea75c21ac1 121 } // end if(Channels connected)
jdawkins 0:daea75c21ac1 122
jdawkins 2:2bb375ab3b2b 123 /* if( (abs(CH1.servoPulse-1500) > 200) || (abs(CH2.servoPulse-1500) > 200)){
jdawkins 0:daea75c21ac1 124 auto_flag = 0;
jdawkins 2:2bb375ab3b2b 125 }*/
jdawkins 0:daea75c21ac1 126 }
jdawkins 0:daea75c21ac1 127 void readKeyboard(){
jdawkins 0:daea75c21ac1 128 if(xbee.readable()) {
jdawkins 0:daea75c21ac1 129 char c = xbee.getc();
jdawkins 0:daea75c21ac1 130 if(c == 'H' || c == 'h') {
jdawkins 0:daea75c21ac1 131 xbee.printf("Command List...\r\n");
jdawkins 2:2bb375ab3b2b 132 xbee.printf("d - set open loop duty cycle\r\n");
jdawkins 2:2bb375ab3b2b 133 xbee.printf("r - run open loop control with current duty cycle\r\n");
jdawkins 2:2bb375ab3b2b 134 xbee.printf("h - display this prompt and exit\r\n");
jdawkins 2:2bb375ab3b2b 135 xbee.printf("t - set duration of test\r\n\n\n");
jdawkins 0:daea75c21ac1 136 xbee.printf("Enter Command\r\n");
jdawkins 0:daea75c21ac1 137 }
jdawkins 0:daea75c21ac1 138
jdawkins 0:daea75c21ac1 139 if(c == 'R' || c == 'r') {
jdawkins 0:daea75c21ac1 140 auto_flag = true;
jdawkins 1:f4c9926fb4c9 141 log_flag = true;
jdawkins 0:daea75c21ac1 142 initializeControl();
jdawkins 0:daea75c21ac1 143 }
jdawkins 0:daea75c21ac1 144 if(c == 'S' || c == 's') {
jdawkins 0:daea75c21ac1 145 auto_flag = false;
jdawkins 1:f4c9926fb4c9 146 log_flag = false;
jdawkins 2:2bb375ab3b2b 147 }
jdawkins 0:daea75c21ac1 148
jdawkins 2:2bb375ab3b2b 149 if(c == 'L' || c == 'l') {
jdawkins 2:2bb375ab3b2b 150 log_flag = !log_flag;
jdawkins 2:2bb375ab3b2b 151 }
jdawkins 0:daea75c21ac1 152 }
jdawkins 0:daea75c21ac1 153 }
jdawkins 0:daea75c21ac1 154
jdawkins 0:daea75c21ac1 155
jdawkins 0:daea75c21ac1 156
jdawkins 0:daea75c21ac1 157 int main()
jdawkins 0:daea75c21ac1 158 {
jdawkins 0:daea75c21ac1 159
jdawkins 0:daea75c21ac1 160 log_it = 0;
jdawkins 0:daea75c21ac1 161 auto_mode = 0;
jdawkins 0:daea75c21ac1 162 // Default Steering and Throttle commands to neutral
jdawkins 0:daea75c21ac1 163 Steer.write(1500); // Write Steering Pulse
jdawkins 0:daea75c21ac1 164 Throttle.write(1500);
jdawkins 0:daea75c21ac1 165
jdawkins 0:daea75c21ac1 166
jdawkins 0:daea75c21ac1 167 t.start();
jdawkins 0:daea75c21ac1 168 t_start = t.read();
jdawkins 0:daea75c21ac1 169 t_log = t.read();
jdawkins 0:daea75c21ac1 170 t_ctrl = t.read();
jdawkins 0:daea75c21ac1 171 t_5hz = t.read();
jdawkins 0:daea75c21ac1 172
jdawkins 0:daea75c21ac1 173 while(1) {
jdawkins 0:daea75c21ac1 174 auto_LED = auto_flag;
jdawkins 0:daea75c21ac1 175 dwm_LED = rc_conn;
jdawkins 0:daea75c21ac1 176 readKeyboard(); // Check for Input from Keyboard over xbee
jdawkins 0:daea75c21ac1 177
jdawkins 0:daea75c21ac1 178 // check for servo pulse from either channel of receiver module
jdawkins 0:daea75c21ac1 179 readRCSignal();
jdawkins 0:daea75c21ac1 180
jdawkins 2:2bb375ab3b2b 181
jdawkins 2:2bb375ab3b2b 182
jdawkins 2:2bb375ab3b2b 183
jdawkins 2:2bb375ab3b2b 184
jdawkins 0:daea75c21ac1 185 if(t.read()-t_5hz > (1.0/FIVE_HZ_LOOP)) {
jdawkins 0:daea75c21ac1 186 // 5 Hz loop intended for debugging print statements
jdawkins 0:daea75c21ac1 187
jdawkins 2:2bb375ab3b2b 188 // xbee.printf("Auto Cond: %f , %f \r\n",abs(CH1.servoPulse-1500),abs(CH2.servoPulse-1500));
jdawkins 2:2bb375ab3b2b 189 t_5hz = t.read();
jdawkins 2:2bb375ab3b2b 190
jdawkins 0:daea75c21ac1 191 }
jdawkins 0:daea75c21ac1 192
jdawkins 0:daea75c21ac1 193 if(t.read()-t_ctrl > (1.0/CTRL_LOOP_FREQ)) {
jdawkins 0:daea75c21ac1 194 // 100 Hz loop for calling control loop
jdawkins 0:daea75c21ac1 195 controlLoop();
jdawkins 0:daea75c21ac1 196 t_ctrl = t.read();
jdawkins 0:daea75c21ac1 197 }
jdawkins 0:daea75c21ac1 198
jdawkins 0:daea75c21ac1 199
jdawkins 0:daea75c21ac1 200
jdawkins 0:daea75c21ac1 201 if(t.read()-t_log > (1.0/LOG_LOOP_FREQ)) {
jdawkins 0:daea75c21ac1 202 // Faster loop for logging data for plotting and analysis
jdawkins 2:2bb375ab3b2b 203
jdawkins 2:2bb375ab3b2b 204 mp.getAccel();
jdawkins 2:2bb375ab3b2b 205 mp.getGyro();
jdawkins 2:2bb375ab3b2b 206 mp.getMag();
jdawkins 2:2bb375ab3b2b 207 mp.getEuler();
jdawkins 2:2bb375ab3b2b 208 mp.getSpeed();
jdawkins 2:2bb375ab3b2b 209 //xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);
jdawkins 2:2bb375ab3b2b 210
jdawkins 2:2bb375ab3b2b 211 if(log_flag){
jdawkins 2:2bb375ab3b2b 212 dataLog();
jdawkins 2:2bb375ab3b2b 213 }
jdawkins 0:daea75c21ac1 214 t_log = t.read();
jdawkins 0:daea75c21ac1 215 }
jdawkins 0:daea75c21ac1 216
jdawkins 0:daea75c21ac1 217 if(rc_conn) {
jdawkins 0:daea75c21ac1 218 mp.setSteer(str);
jdawkins 0:daea75c21ac1 219 mp.setThrottle(thr);
jdawkins 0:daea75c21ac1 220
jdawkins 0:daea75c21ac1 221 } else {
jdawkins 0:daea75c21ac1 222 Steer.write(1500); // Write Steering Pulse
jdawkins 0:daea75c21ac1 223 Throttle.write(1500);
jdawkins 0:daea75c21ac1 224 }
jdawkins 0:daea75c21ac1 225
jdawkins 0:daea75c21ac1 226 wait(1.0/MAIN_LOOP_FREQ);
jdawkins 0:daea75c21ac1 227 } // End While 1 Loop
jdawkins 0:daea75c21ac1 228 } // End Main Function
jdawkins 0:daea75c21ac1 229
jdawkins 0:daea75c21ac1 230 float wrapTo2pi(float ang)
jdawkins 0:daea75c21ac1 231 {
jdawkins 0:daea75c21ac1 232 if(ang > 2*PI) {
jdawkins 0:daea75c21ac1 233 ang = ang - 2*PI;
jdawkins 0:daea75c21ac1 234 }
jdawkins 0:daea75c21ac1 235
jdawkins 0:daea75c21ac1 236 if(ang < 0) {
jdawkins 0:daea75c21ac1 237 ang = ang + 2*PI;
jdawkins 0:daea75c21ac1 238 }
jdawkins 0:daea75c21ac1 239 return ang;
jdawkins 0:daea75c21ac1 240 }
jdawkins 0:daea75c21ac1 241
jdawkins 0:daea75c21ac1 242 float wrapTopi(float ang)
jdawkins 0:daea75c21ac1 243 {
jdawkins 0:daea75c21ac1 244 if(ang > PI) {
jdawkins 0:daea75c21ac1 245 ang = ang - 2*PI;
jdawkins 0:daea75c21ac1 246 }
jdawkins 0:daea75c21ac1 247
jdawkins 0:daea75c21ac1 248 if(ang < -PI) {
jdawkins 0:daea75c21ac1 249 ang = ang + 2*PI;
jdawkins 0:daea75c21ac1 250 }
jdawkins 0:daea75c21ac1 251 return ang;
jdawkins 2:2bb375ab3b2b 252 }