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@23:55f8b1abbf99, 2015-08-05 (annotated)

Committer:
zsunberg
Date:
Wed Aug 05 05:21:41 2015 +0000
Revision:
23:55f8b1abbf99
Parent:
22:dae192ffca90
Child:
24:62cad0ea47da
before comms refactor

Who changed what in which revision?

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