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@29:6aa49bba0d81, 2015-08-05 (annotated)

Committer:
Zachary Sunberg
Date:
Wed Aug 05 19:42:25 2015 -0700
Revision:
29:6aa49bba0d81
Parent:
28:15b95329a064
Child:
30:ea511cd81f43
added pid back

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