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@30:ea511cd81f43, 2015-08-06 (annotated)

Committer:
zsunberg
Date:
Thu Aug 06 06:30:50 2015 +0000
Revision:
30:ea511cd81f43
Parent:
29:6aa49bba0d81
Child:
31:e36b7722df56
pid working

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