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@19:c900c40b270e, 2015-07-22 (annotated)

Committer:
laurennyg
Date:
Wed Jul 22 22:00:05 2015 +0000
Revision:
19:c900c40b270e
Parent:
18:f43f482ddede
Child:
20:f0ca65974329
updated line following;

Who changed what in which revision?

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