USNA WSE ES456 / Mbed 2 deprecated Madpulse_Speed_Control_temp

Push for Students

Dependencies:   BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed

Fork of Madpulse_Control_Fall2017 by USNA WSE ES456

madpulse_ctrl.cpp@1:f4c9926fb4c9, 2017-11-15 (annotated)

Committer:
jdawkins
Date:
Wed Nov 15 18:36:30 2017 +0000
Revision:
1:f4c9926fb4c9
Parent:
0:daea75c21ac1 
Committ for class

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 1:f4c9926fb4c9 36 ///////////////////////////////////////////////
jdawkins 1:f4c9926fb4c9 37 // Define Control Gains and Controller Variable
jdawkins 1:f4c9926fb4c9 38 //////////////////////////////////////////////
jdawkins 0:daea75c21ac1 39
jdawkins 0:daea75c21ac1 40
jdawkins 0:daea75c21ac1 41 float psi_est,psi_est_old;
jdawkins 0:daea75c21ac1 42 float spd_err,spd_err_i;
jdawkins 0:daea75c21ac1 43 float spd,des_spd;
jdawkins 0:daea75c21ac1 44 float dist;
jdawkins 0:daea75c21ac1 45
jdawkins 0:daea75c21ac1 46 Timer t; // Create Timer
jdawkins 0:daea75c21ac1 47 int log_it; // Iterator for Spacing logging of different messages
jdawkins 0:daea75c21ac1 48
jdawkins 0:daea75c21ac1 49 uint8_t buf[BUFFER_SIZE];
jdawkins 0:daea75c21ac1 50
jdawkins 0:daea75c21ac1 51 float t_start,t_log,t_ctrl,t_5hz,t_run; // Create timer variables
jdawkins 0:daea75c21ac1 52 float dt;
jdawkins 0:daea75c21ac1 53 float str,thr,wheel_spd;
jdawkins 0:daea75c21ac1 54 float wrapTo2pi(float ang);
jdawkins 0:daea75c21ac1 55 float wrapTopi(float ang);
jdawkins 1:f4c9926fb4c9 56 bool rc_conn,auto_flag,log_flag;
jdawkins 1:f4c9926fb4c9 57
jdawkins 1:f4c9926fb4c9 58
jdawkins 1:f4c9926fb4c9 59 void controlLoop(){
jdawkins 1:f4c9926fb4c9 60 dt = t.read()-t_ctrl;
jdawkins 1:f4c9926fb4c9 61
jdawkins 1:f4c9926fb4c9 62 if(auto_flag){ // If vehicle is in Auto Mode
jdawkins 1:f4c9926fb4c9 63
jdawkins 1:f4c9926fb4c9 64 spd = mp.getFreq()/27.8119; // Convert from counts per second to meters/second
jdawkins 1:f4c9926fb4c9 65
jdawkins 1:f4c9926fb4c9 66 str = 0.001;
jdawkins 1:f4c9926fb4c9 67 if(t.read()-t_run < 1){
jdawkins 1:f4c9926fb4c9 68
jdawkins 1:f4c9926fb4c9 69 thr = 0.1; // Set throttle, range (-1 to 1)
jdawkins 1:f4c9926fb4c9 70 }else{
jdawkins 1:f4c9926fb4c9 71 thr = 0.0;
jdawkins 1:f4c9926fb4c9 72 }
jdawkins 1:f4c9926fb4c9 73 }else{
jdawkins 1:f4c9926fb4c9 74 str = ((CH1.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 1:f4c9926fb4c9 75 thr = ((CH2.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 1:f4c9926fb4c9 76 }
jdawkins 1:f4c9926fb4c9 77
jdawkins 1:f4c9926fb4c9 78 t_ctrl = t.read();
jdawkins 1:f4c9926fb4c9 79 }
jdawkins 1:f4c9926fb4c9 80
jdawkins 1:f4c9926fb4c9 81 void logData(){
jdawkins 1:f4c9926fb4c9 82 // Accel Data: mp.accel.x,mp.accel.y,mp.accel.z
jdawkins 1:f4c9926fb4c9 83 // Gyro Data: mp.gyro.x,mp.gyro.y,mp.gyro.z
jdawkins 1:f4c9926fb4c9 84 // Mag Data: mp.mag.x,mp.mag.y,mp.mag.z
jdawkins 1:f4c9926fb4c9 85 // Euler Data: mp.euler.roll,mp.euler.pitch, mp.euler.yaw
jdawkins 1:f4c9926fb4c9 86 // Encoder Data: mp.getCounts(), mp.getFreq()
jdawkins 1:f4c9926fb4c9 87
jdawkins 1:f4c9926fb4c9 88
jdawkins 1:f4c9926fb4c9 89 xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);
jdawkins 1:f4c9926fb4c9 90 }
jdawkins 1:f4c9926fb4c9 91
jdawkins 0:daea75c21ac1 92
jdawkins 0:daea75c21ac1 93 void initializeControl(){
jdawkins 0:daea75c21ac1 94 printf("Initialize Control\r\n");
jdawkins 0:daea75c21ac1 95 t_run = t.read();
jdawkins 0:daea75c21ac1 96 }
jdawkins 0:daea75c21ac1 97
jdawkins 0:daea75c21ac1 98 void stopControl(){
jdawkins 0:daea75c21ac1 99 printf("Stop Control\r\n");
jdawkins 0:daea75c21ac1 100 }
jdawkins 1:f4c9926fb4c9 101
jdawkins 0:daea75c21ac1 102 void readRCSignal(){
jdawkins 0:daea75c21ac1 103 if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
jdawkins 0:daea75c21ac1 104 rc_conn = false;
jdawkins 0:daea75c21ac1 105 } else {
jdawkins 0:daea75c21ac1 106 rc_conn = true;
jdawkins 0:daea75c21ac1 107 } // end if(Channels connected)
jdawkins 0:daea75c21ac1 108
jdawkins 0:daea75c21ac1 109 if( (abs(CH1.servoPulse-1500) > 200) || (abs(CH2.servoPulse-1500) > 200)){
jdawkins 0:daea75c21ac1 110 auto_flag = 0;
jdawkins 0:daea75c21ac1 111 }
jdawkins 0:daea75c21ac1 112 }
jdawkins 0:daea75c21ac1 113 void readKeyboard(){
jdawkins 0:daea75c21ac1 114 if(xbee.readable()) {
jdawkins 0:daea75c21ac1 115 char c = xbee.getc();
jdawkins 0:daea75c21ac1 116 if(c == 'H' || c == 'h') {
jdawkins 0:daea75c21ac1 117 xbee.printf("Command List...\r\n");
jdawkins 1:f4c9926fb4c9 118 xbee.printf("h - Display this prompt and return\r\n");
jdawkins 1:f4c9926fb4c9 119 xbee.printf("l - Toggle Logging\r\n");
jdawkins 1:f4c9926fb4c9 120 xbee.printf("r - Run, switch vehicle to auto mode\r\n");
jdawkins 1:f4c9926fb4c9 121 xbee.printf("s - Stop, stop vehicle autor control\r\n");
jdawkins 0:daea75c21ac1 122 xbee.printf("Enter Command\r\n");
jdawkins 0:daea75c21ac1 123 }
jdawkins 0:daea75c21ac1 124
jdawkins 1:f4c9926fb4c9 125 if(c == 'L' || c == 'l') {
jdawkins 1:f4c9926fb4c9 126 log_flag = !log_flag; //toggle logging flag
jdawkins 1:f4c9926fb4c9 127
jdawkins 1:f4c9926fb4c9 128 }
jdawkins 0:daea75c21ac1 129 if(c == 'R' || c == 'r') {
jdawkins 0:daea75c21ac1 130 auto_flag = true;
jdawkins 1:f4c9926fb4c9 131 log_flag = true;
jdawkins 0:daea75c21ac1 132 initializeControl();
jdawkins 0:daea75c21ac1 133 }
jdawkins 0:daea75c21ac1 134 if(c == 'S' || c == 's') {
jdawkins 0:daea75c21ac1 135 auto_flag = false;
jdawkins 1:f4c9926fb4c9 136 log_flag = false;
jdawkins 1:f4c9926fb4c9 137 stopControl();
jdawkins 1:f4c9926fb4c9 138 }
jdawkins 0:daea75c21ac1 139
jdawkins 0:daea75c21ac1 140 }
jdawkins 0:daea75c21ac1 141 }
jdawkins 0:daea75c21ac1 142
jdawkins 0:daea75c21ac1 143
jdawkins 0:daea75c21ac1 144
jdawkins 0:daea75c21ac1 145 int main()
jdawkins 0:daea75c21ac1 146 {
jdawkins 0:daea75c21ac1 147
jdawkins 0:daea75c21ac1 148 log_it = 0;
jdawkins 0:daea75c21ac1 149 auto_mode = 0;
jdawkins 0:daea75c21ac1 150 // Default Steering and Throttle commands to neutral
jdawkins 0:daea75c21ac1 151 Steer.write(1500); // Write Steering Pulse
jdawkins 0:daea75c21ac1 152 Throttle.write(1500);
jdawkins 0:daea75c21ac1 153
jdawkins 0:daea75c21ac1 154
jdawkins 0:daea75c21ac1 155 t.start();
jdawkins 0:daea75c21ac1 156 t_start = t.read();
jdawkins 0:daea75c21ac1 157 t_log = t.read();
jdawkins 0:daea75c21ac1 158 t_ctrl = t.read();
jdawkins 0:daea75c21ac1 159 t_5hz = t.read();
jdawkins 0:daea75c21ac1 160
jdawkins 0:daea75c21ac1 161 while(1) {
jdawkins 0:daea75c21ac1 162 auto_LED = auto_flag;
jdawkins 0:daea75c21ac1 163 dwm_LED = rc_conn;
jdawkins 0:daea75c21ac1 164 readKeyboard(); // Check for Input from Keyboard over xbee
jdawkins 0:daea75c21ac1 165
jdawkins 0:daea75c21ac1 166 // check for servo pulse from either channel of receiver module
jdawkins 0:daea75c21ac1 167 readRCSignal();
jdawkins 0:daea75c21ac1 168
jdawkins 0:daea75c21ac1 169 if(t.read()-t_5hz > (1.0/FIVE_HZ_LOOP)) {
jdawkins 0:daea75c21ac1 170 // 5 Hz loop intended for debugging print statements
jdawkins 0:daea75c21ac1 171
jdawkins 1:f4c9926fb4c9 172 t_5hz = t.read();
jdawkins 0:daea75c21ac1 173 }
jdawkins 0:daea75c21ac1 174
jdawkins 0:daea75c21ac1 175 if(t.read()-t_ctrl > (1.0/CTRL_LOOP_FREQ)) {
jdawkins 0:daea75c21ac1 176 // 100 Hz loop for calling control loop
jdawkins 1:f4c9926fb4c9 177 mp.getAccel();
jdawkins 1:f4c9926fb4c9 178 mp.getGyro();
jdawkins 1:f4c9926fb4c9 179 mp.getMag();
jdawkins 1:f4c9926fb4c9 180 mp.getEuler();
jdawkins 1:f4c9926fb4c9 181 mp.getSpeed();
jdawkins 0:daea75c21ac1 182 controlLoop();
jdawkins 0:daea75c21ac1 183 t_ctrl = t.read();
jdawkins 0:daea75c21ac1 184 }
jdawkins 0:daea75c21ac1 185
jdawkins 0:daea75c21ac1 186
jdawkins 0:daea75c21ac1 187
jdawkins 0:daea75c21ac1 188 if(t.read()-t_log > (1.0/LOG_LOOP_FREQ)) {
jdawkins 0:daea75c21ac1 189 // Faster loop for logging data for plotting and analysis
jdawkins 1:f4c9926fb4c9 190 if(log_flag){
jdawkins 1:f4c9926fb4c9 191 logData();
jdawkins 1:f4c9926fb4c9 192 }
jdawkins 0:daea75c21ac1 193 t_log = t.read();
jdawkins 0:daea75c21ac1 194 }
jdawkins 0:daea75c21ac1 195
jdawkins 0:daea75c21ac1 196 if(rc_conn) {
jdawkins 0:daea75c21ac1 197 // str = ((CH1.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:daea75c21ac1 198 // thr = ((CH2.servoPulse-1500.0)/600.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:daea75c21ac1 199 mp.setSteer(str);
jdawkins 0:daea75c21ac1 200 mp.setThrottle(thr);
jdawkins 0:daea75c21ac1 201
jdawkins 0:daea75c21ac1 202 } else {
jdawkins 0:daea75c21ac1 203 Steer.write(1500); // Write Steering Pulse
jdawkins 0:daea75c21ac1 204 Throttle.write(1500);
jdawkins 0:daea75c21ac1 205 // mp.setSteer(str);
jdawkins 0:daea75c21ac1 206 // mp.setThrottle(thr);
jdawkins 0:daea75c21ac1 207 }
jdawkins 0:daea75c21ac1 208
jdawkins 0:daea75c21ac1 209 wait(1.0/MAIN_LOOP_FREQ);
jdawkins 0:daea75c21ac1 210 } // End While 1 Loop
jdawkins 0:daea75c21ac1 211 } // End Main Function
jdawkins 0:daea75c21ac1 212
jdawkins 0:daea75c21ac1 213 float wrapTo2pi(float ang)
jdawkins 0:daea75c21ac1 214 {
jdawkins 0:daea75c21ac1 215 if(ang > 2*PI) {
jdawkins 0:daea75c21ac1 216 ang = ang - 2*PI;
jdawkins 0:daea75c21ac1 217 }
jdawkins 0:daea75c21ac1 218
jdawkins 0:daea75c21ac1 219 if(ang < 0) {
jdawkins 0:daea75c21ac1 220 ang = ang + 2*PI;
jdawkins 0:daea75c21ac1 221 }
jdawkins 0:daea75c21ac1 222 return ang;
jdawkins 0:daea75c21ac1 223 }
jdawkins 0:daea75c21ac1 224
jdawkins 0:daea75c21ac1 225 float wrapTopi(float ang)
jdawkins 0:daea75c21ac1 226 {
jdawkins 0:daea75c21ac1 227 if(ang > PI) {
jdawkins 0:daea75c21ac1 228 ang = ang - 2*PI;
jdawkins 0:daea75c21ac1 229 }
jdawkins 0:daea75c21ac1 230
jdawkins 0:daea75c21ac1 231 if(ang < -PI) {
jdawkins 0:daea75c21ac1 232 ang = ang + 2*PI;
jdawkins 0:daea75c21ac1 233 }
jdawkins 0:daea75c21ac1 234 return ang;
jdawkins 0:daea75c21ac1 235 }
jdawkins 0:daea75c21ac1 236