Zachary Sunberg / Mbed 2 deprecated SAILORSbot

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

main.cpp@37:c31268270bb2, 2016-07-13 (annotated)

Committer:
zsunberg
Date:
Wed Jul 13 19:48:12 2016 +0000
Revision:
37:c31268270bb2
Parent:
35:09f92a88c0a7 
put pid code back in

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