Zachary Sunberg / Mbed 2 deprecated SAILORSbot_student

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of SAILORSbot by Zachary Sunberg

main.cpp@31:e36b7722df56, 2015-08-07 (annotated)

Committer:
zsunberg
Date:
Fri Aug 07 05:34:13 2015 +0000
Revision:
31:e36b7722df56
Parent:
30:ea511cd81f43
Child:
32:7e518320305f
added nudge

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zsunberg 0:9f058fe8cab5 1 #include "robot.h"
zsunberg 31:e36b7722df56 2 #include "turn.h"
zsunberg 31:e36b7722df56 3 #include "nudge.h"
zsunberg 23:55f8b1abbf99 4 #include "my_functions.h"
zsunberg 0:9f058fe8cab5 5
zsunberg 1:a7aab5937375 6 /*
zsunberg 30:ea511cd81f43 7 * This function will be called at approximately 20 hz when the control mode is LINE_FOLLOW_MODE
Zachary Sunberg 4:70fea94b29ae 8 */
Zachary Sunberg 5:70c86dbc8832 9 void line_follow_loop(){
Zachary Sunberg 4:70fea94b29ae 10 led4 = 1;
laurennyg 19:c900c40b270e 11
zsunberg 31:e36b7722df56 12 // Zach's control code
zsunberg 31:e36b7722df56 13 //////////////////////
zsunberg 30:ea511cd81f43 14 static double previous_position = 0.0;
zsunberg 30:ea511cd81f43 15 static double integral = 0.0;
zsunberg 30:ea511cd81f43 16
zsunberg 30:ea511cd81f43 17 double derivative = (line_position - previous_position);
zsunberg 30:ea511cd81f43 18 integral += line_position;
zsunberg 30:ea511cd81f43 19 previous_position = line_position;
Zachary Sunberg 29:6aa49bba0d81 20
zsunberg 30:ea511cd81f43 21 double delta = k_p*line_position + k_i*integral + k_d*derivative;
Zachary Sunberg 29:6aa49bba0d81 22
Zachary Sunberg 29:6aa49bba0d81 23 leftspeed = speed+delta;
Zachary Sunberg 29:6aa49bba0d81 24 rightspeed = speed-delta;
zsunberg 28:15b95329a064 25
zsunberg 30:ea511cd81f43 26 if(leftspeed > MAX){
zsunberg 30:ea511cd81f43 27 leftspeed = MAX;
zsunberg 30:ea511cd81f43 28 }
zsunberg 30:ea511cd81f43 29 if(rightspeed > MAX){
zsunberg 30:ea511cd81f43 30 rightspeed = MAX;
zsunberg 30:ea511cd81f43 31 }
zsunberg 30:ea511cd81f43 32 if(leftspeed < MIN){
zsunberg 30:ea511cd81f43 33 leftspeed = MIN;
zsunberg 30:ea511cd81f43 34 }
zsunberg 30:ea511cd81f43 35 if(rightspeed < MIN){
zsunberg 30:ea511cd81f43 36 rightspeed = MIN;
zsunberg 30:ea511cd81f43 37 }
zsunberg 31:e36b7722df56 38 /////////////////////
zsunberg 31:e36b7722df56 39
zsunberg 30:ea511cd81f43 40
zsunberg 27:949a31d30439 41 // Do your control calculations here
zsunberg 27:949a31d30439 42 ////////////////////////////////////
laurennyg 19:c900c40b270e 43
zsunberg 27:949a31d30439 44
zsunberg 27:949a31d30439 45 // set motor speeds here
zsunberg 27:949a31d30439 46 // leftspeed = ???;
zsunberg 27:949a31d30439 47 // rightspeed = ???;
zsunberg 27:949a31d30439 48 ////////////////////////////////////
laurennyg 19:c900c40b270e 49
zsunberg 31:e36b7722df56 50
zsunberg 31:e36b7722df56 51 // Code to check if we need to stop
zsunberg 31:e36b7722df56 52 if(sensors[0] < 1200 && sensors[1] < 1200 && sensors[2] < 1200 && sensors[3] < 1200 && sensors[4] < 1200){
zsunberg 31:e36b7722df56 53 //dead end
zsunberg 31:e36b7722df56 54 leftspeed = 0.0;
zsunberg 31:e36b7722df56 55 rightspeed = 0.0;
zsunberg 31:e36b7722df56 56 pi.stop();
zsunberg 31:e36b7722df56 57 mode = MANUAL_MODE;
zsunberg 31:e36b7722df56 58 }else if(sensors[0] > 1800 || sensors[4] > 1800){
zsunberg 31:e36b7722df56 59 //intersection
zsunberg 31:e36b7722df56 60 leftspeed = 0.0;
zsunberg 31:e36b7722df56 61 rightspeed = 0.0;
zsunberg 31:e36b7722df56 62 pi.stop();
zsunberg 31:e36b7722df56 63 mode = MANUAL_MODE;
zsunberg 31:e36b7722df56 64 }
zsunberg 11:f0df5dea322d 65
Zachary Sunberg 4:70fea94b29ae 66 led4 = 0;
Zachary Sunberg 4:70fea94b29ae 67 }
Zachary Sunberg 4:70fea94b29ae 68
Zachary Sunberg 4:70fea94b29ae 69
Zachary Sunberg 4:70fea94b29ae 70 // INPUT COMMANDS
Zachary Sunberg 4:70fea94b29ae 71 // l:<float> set left wheel speed (only effective in MANUAL_MODE)
Zachary Sunberg 4:70fea94b29ae 72 // r:<float> set right wheel speed (only effective in MANUAL_MODE)
Zachary Sunberg 4:70fea94b29ae 73 // c:<int> change mode
Zachary Sunberg 29:6aa49bba0d81 74 // g:<p|i|d|s>:<float>
Zachary Sunberg 29:6aa49bba0d81 75 // change gains/speed
zsunberg 22:dae192ffca90 76 // b: check battery
zsunberg 31:e36b7722df56 77 // t:<l|r> turn
zsunberg 31:e36b7722df56 78 // n:<float> nudge
Zachary Sunberg 4:70fea94b29ae 79
Zachary Sunberg 4:70fea94b29ae 80 // OUTPUT MESSAGES
Zachary Sunberg 4:70fea94b29ae 81 // p:<float> line position
Zachary Sunberg 4:70fea94b29ae 82 // s:<int>,<int>,<int>,<int>,<int>
Zachary Sunberg 4:70fea94b29ae 83 // light sensor values
Zachary Sunberg 4:70fea94b29ae 84 // m:<int> mode
zsunberg 23:55f8b1abbf99 85 // a:<message> acknowledge
zsunberg 22:dae192ffca90 86 void communicate()
zsunberg 22:dae192ffca90 87 {
zsunberg 22:dae192ffca90 88 led1 = 1;
zsunberg 22:dae192ffca90 89 int* s = sensors; // just to make the next line more compact
zsunberg 22:dae192ffca90 90 xbee.printf("s:%i,%i,%i,%i,%i\n", s[0], s[1], s[2], s[3], s[4]);
zsunberg 30:ea511cd81f43 91 xbee.printf("p:%f\n", line_position);
zsunberg 22:dae192ffca90 92 xbee.printf("m:%d\n", mode);
Zachary Sunberg 29:6aa49bba0d81 93 // send any other variables here
Zachary Sunberg 29:6aa49bba0d81 94 ////////////////////////////////
Zachary Sunberg 29:6aa49bba0d81 95
Zachary Sunberg 29:6aa49bba0d81 96 ////////////////////////////////
zsunberg 22:dae192ffca90 97 led1 = 0;
zsunberg 22:dae192ffca90 98 }
Zachary Sunberg 4:70fea94b29ae 99
Zachary Sunberg 25:c4577daa425a 100 void signal_comm(){
Zachary Sunberg 25:c4577daa425a 101 comm_time = true;
Zachary Sunberg 25:c4577daa425a 102 }
Zachary Sunberg 25:c4577daa425a 103
zsunberg 1:a7aab5937375 104 int parse_command(const char* cmd)
zsunberg 1:a7aab5937375 105 {
zsunberg 1:a7aab5937375 106 if(cmd[1]==':'){
Zachary Sunberg 12:f96af1d15e9e 107 // left motor
Zachary Sunberg 9:7f94942cd50d 108 if(cmd[0]=='l'){
Zachary Sunberg 9:7f94942cd50d 109 if(mode==MANUAL_MODE){
Zachary Sunberg 9:7f94942cd50d 110 leftspeed = atof(&cmd[2]);
Zachary Sunberg 9:7f94942cd50d 111 }
Zachary Sunberg 12:f96af1d15e9e 112 // right motor
Zachary Sunberg 9:7f94942cd50d 113 }else if(cmd[0]=='r'){
Zachary Sunberg 9:7f94942cd50d 114 if(mode==MANUAL_MODE){
Zachary Sunberg 9:7f94942cd50d 115 rightspeed = atof(&cmd[2]);
Zachary Sunberg 9:7f94942cd50d 116 }
Zachary Sunberg 12:f96af1d15e9e 117 // mode
Zachary Sunberg 4:70fea94b29ae 118 }else if(cmd[0]=='c'){
Zachary Sunberg 4:70fea94b29ae 119 mode = atoi(&cmd[2]);
zsunberg 30:ea511cd81f43 120 __disable_irq();
zsunberg 23:55f8b1abbf99 121 xbee.printf("a:c:%d\n", mode);// acknowledge the mode change
zsunberg 30:ea511cd81f43 122 __enable_irq();
zsunberg 21:18b585a44155 123 // xbee.printf("mode set to %d\n", mode);
Zachary Sunberg 12:f96af1d15e9e 124 // gains
Zachary Sunberg 12:f96af1d15e9e 125 }else if(cmd[0]=='g'){
zsunberg 30:ea511cd81f43 126 __disable_irq();
Zachary Sunberg 12:f96af1d15e9e 127 if(cmd[2]=='p'){
Zachary Sunberg 12:f96af1d15e9e 128 k_p = atof(&cmd[4]);
zsunberg 30:ea511cd81f43 129 xbee.printf("k_p: %f\n", k_p);
Zachary Sunberg 12:f96af1d15e9e 130 }else if(cmd[2]=='i'){
Zachary Sunberg 12:f96af1d15e9e 131 k_i = atof(&cmd[4]);
zsunberg 30:ea511cd81f43 132 xbee.printf("k_i: %f\n", k_i);
Zachary Sunberg 12:f96af1d15e9e 133 }else if(cmd[2]=='d'){
Zachary Sunberg 12:f96af1d15e9e 134 k_d = atof(&cmd[4]);
zsunberg 30:ea511cd81f43 135 xbee.printf("k_d: %f\n", k_d);
Zachary Sunberg 29:6aa49bba0d81 136 }else if(cmd[2]=='s'){
Zachary Sunberg 29:6aa49bba0d81 137 speed = atof(&cmd[4]);
zsunberg 30:ea511cd81f43 138 xbee.printf("speed: %f\n", speed);
Zachary Sunberg 12:f96af1d15e9e 139 }
zsunberg 30:ea511cd81f43 140 __enable_irq();
Zachary Sunberg 29:6aa49bba0d81 141 // xbee.printf("gains p:%f, i:%f, d:%f\n", k_p, k_i, k_d);
zsunberg 22:dae192ffca90 142 // battery
zsunberg 22:dae192ffca90 143 }else if(cmd[0]=='b'){
zsunberg 23:55f8b1abbf99 144 __disable_irq();
zsunberg 22:dae192ffca90 145 xbee.printf("battery voltage: %f\n", pi.battery());
zsunberg 23:55f8b1abbf99 146 __enable_irq();
zsunberg 31:e36b7722df56 147 // turn
zsunberg 31:e36b7722df56 148 }else if(cmd[0]=='t'){
zsunberg 31:e36b7722df56 149 float dir = 0.0;
zsunberg 31:e36b7722df56 150 if(cmd[2] == 'l'){
zsunberg 31:e36b7722df56 151 dir = 1.0;
zsunberg 31:e36b7722df56 152 }else{
zsunberg 31:e36b7722df56 153 dir = -1.0;
zsunberg 31:e36b7722df56 154 }
zsunberg 31:e36b7722df56 155 start_turn(dir);
zsunberg 31:e36b7722df56 156 __disable_irq();
zsunberg 31:e36b7722df56 157 xbee.printf("a:c:%d\n", TURN_MODE);
zsunberg 31:e36b7722df56 158 __enable_irq();
zsunberg 31:e36b7722df56 159 }else if(cmd[0]=='n'){
zsunberg 31:e36b7722df56 160 start_nudge(atof(&cmd[2]));
zsunberg 31:e36b7722df56 161 __disable_irq();
zsunberg 31:e36b7722df56 162 xbee.printf("a:c:%d\n", NUDGE_MODE);
zsunberg 31:e36b7722df56 163 __enable_irq();
zsunberg 31:e36b7722df56 164
Zachary Sunberg 29:6aa49bba0d81 165 // parse your own commands here
Zachary Sunberg 29:6aa49bba0d81 166 ///////////////////////////////
Zachary Sunberg 29:6aa49bba0d81 167
Zachary Sunberg 29:6aa49bba0d81 168 ///////////////////////////////
zsunberg 1:a7aab5937375 169 }else{
zsunberg 30:ea511cd81f43 170 //XXX disabling this is disabling error checking
zsunberg 23:55f8b1abbf99 171 xbee.printf("%s\n",cmd);
zsunberg 1:a7aab5937375 172 }
zsunberg 1:a7aab5937375 173 }else{
zsunberg 30:ea511cd81f43 174 //XXX disabling this is disabling error checking
zsunberg 1:a7aab5937375 175 xbee.printf("%s\n",cmd);
zsunberg 1:a7aab5937375 176 }
zsunberg 1:a7aab5937375 177 return 0;
zsunberg 1:a7aab5937375 178 }
zsunberg 0:9f058fe8cab5 179
Zachary Sunberg 25:c4577daa425a 180 void check_incoming(){
zsunberg 26:49945d96d461 181 char read;
zsunberg 26:49945d96d461 182
zsunberg 0:9f058fe8cab5 183 while(xbee.readable()){
Zachary Sunberg 29:6aa49bba0d81 184 led2 = 1;
zsunberg 1:a7aab5937375 185 read = xbee.getc();
zsunberg 1:a7aab5937375 186 if(read=='\n'){
zsunberg 1:a7aab5937375 187 received[r_index]='\0'; // put a null character at the end
zsunberg 1:a7aab5937375 188 parse_command(received);
zsunberg 1:a7aab5937375 189 r_index=0;
zsunberg 0:9f058fe8cab5 190 } else {
zsunberg 30:ea511cd81f43 191 if(r_index >= 1024){
zsunberg 1:a7aab5937375 192 r_index=0;
zsunberg 1:a7aab5937375 193 }
zsunberg 1:a7aab5937375 194 received[r_index]=read;
zsunberg 1:a7aab5937375 195 r_index++;
zsunberg 0:9f058fe8cab5 196 }
zsunberg 0:9f058fe8cab5 197 }
zsunberg 1:a7aab5937375 198 led2=0;
zsunberg 0:9f058fe8cab5 199 }
zsunberg 0:9f058fe8cab5 200
zsunberg 20:f0ca65974329 201 void control()
zsunberg 20:f0ca65974329 202 {
zsunberg 30:ea511cd81f43 203 pi.sensor_reading(sensors);
zsunberg 30:ea511cd81f43 204 line_position = pi.line_position();
zsunberg 20:f0ca65974329 205 if(mode==LINE_FOLLOW_MODE){
zsunberg 20:f0ca65974329 206 line_follow_loop();
zsunberg 20:f0ca65974329 207 }
Zachary Sunberg 25:c4577daa425a 208 pi.left_motor(rightspeed);
Zachary Sunberg 25:c4577daa425a 209 pi.right_motor(leftspeed);
zsunberg 20:f0ca65974329 210 }
zsunberg 20:f0ca65974329 211
zsunberg 0:9f058fe8cab5 212 int main() {
zsunberg 0:9f058fe8cab5 213
zsunberg 26:49945d96d461 214 // xbee.attach(&Rx_interrupt, Serial::RxIrq);
zsunberg 0:9f058fe8cab5 215 xbeeReset = 0;
zsunberg 0:9f058fe8cab5 216 wait(2);
zsunberg 0:9f058fe8cab5 217 xbeeReset = 1;
zsunberg 1:a7aab5937375 218 pi.sensor_auto_calibrate();
zsunberg 20:f0ca65974329 219 leftspeed = 0.0;
zsunberg 20:f0ca65974329 220 rightspeed = 0.0;
zsunberg 26:49945d96d461 221
zsunberg 1:a7aab5937375 222 r_index = 0;
zsunberg 1:a7aab5937375 223 received[0] = '\0';
zsunberg 7:860856cead84 224 mode = MANUAL_MODE;
zsunberg 20:f0ca65974329 225
Zachary Sunberg 25:c4577daa425a 226 communication.attach(&signal_comm, 0.1);
zsunberg 21:18b585a44155 227 controls.attach(&control, 0.02);
zsunberg 0:9f058fe8cab5 228
Zachary Sunberg 25:c4577daa425a 229 // main loop
zsunberg 0:9f058fe8cab5 230 while(1){
zsunberg 20:f0ca65974329 231 led3 = mode;
Zachary Sunberg 8:9792935abfc3 232
Zachary Sunberg 25:c4577daa425a 233 check_incoming();
Zachary Sunberg 25:c4577daa425a 234
Zachary Sunberg 25:c4577daa425a 235 if(comm_time){
Zachary Sunberg 25:c4577daa425a 236 communicate();
Zachary Sunberg 25:c4577daa425a 237 comm_time = false;
Zachary Sunberg 25:c4577daa425a 238 }
Zachary Sunberg 4:70fea94b29ae 239
zsunberg 0:9f058fe8cab5 240 }
zsunberg 0:9f058fe8cab5 241
zsunberg 0:9f058fe8cab5 242 return 0;
zsunberg 0:9f058fe8cab5 243 }