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@21:18b585a44155, 2015-07-27 (annotated)

Committer:
zsunberg
Date:
Mon Jul 27 22:43:24 2015 +0000
Revision:
21:18b585a44155
Parent:
20:f0ca65974329
Child:
22:dae192ffca90
changed frequencies

Who changed what in which revision?

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