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@28:15b95329a064, 2015-08-06 (annotated)

Committer:
zsunberg
Date:
Thu Aug 06 02:16:53 2015 +0000
Revision:
28:15b95329a064
Parent:
27:949a31d30439
Child:
29:6aa49bba0d81
switching to vim

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