Push for Students
Dependencies: BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed
Fork of Madpulse_Speed_Control_temp by
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?
User | Revision | Line number | New 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 | } |