Zachary Sunberg / Mbed 2 deprecated SAILORSbot_student

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

Fork of SAILORSbot by Zachary Sunberg

main.cpp@15:0e3360eb84cc, 2015-07-21 (annotated)

Committer:
Zachary Sunberg
Date:
Tue Jul 21 15:50:09 2015 -0700
Revision:
15:0e3360eb84cc
Parent:
14:caef8f217015
Child:
16:94857d61e839
echos gain values

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 12: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 12:f96af1d15e9e 15 // GAINS
Zachary Sunberg 4:70fea94b29ae 16 //////////////////////////////
Zachary Sunberg 12:f96af1d15e9e 17 float k_p = 0.0;
Zachary Sunberg 12:f96af1d15e9e 18 float k_i = 0.0;
Zachary Sunberg 12:f96af1d15e9e 19 float k_d = 0.0;
Zachary Sunberg 4:70fea94b29ae 20 //////////////////////////////
zsunberg 0:9f058fe8cab5 21
zsunberg 1:a7aab5937375 22 /*
Zachary Sunberg 4:70fea94b29ae 23 * This function will be called at approximately 20 Hz when the control mode is LINE_FOLLOW_MODE
Zachary Sunberg 4:70fea94b29ae 24 */
Zachary Sunberg 5:70c86dbc8832 25 void line_follow_loop(){
Zachary Sunberg 4:70fea94b29ae 26 led4 = 1;
Zachary Sunberg 4:70fea94b29ae 27
Zachary Sunberg 4:70fea94b29ae 28 // set these variables to control the robot
Zachary Sunberg 4:70fea94b29ae 29 leftspeed = 0.0;
Zachary Sunberg 4:70fea94b29ae 30 rightspeed = 0.0;
Zachary Sunberg 4:70fea94b29ae 31
Zachary Sunberg 4:70fea94b29ae 32 // set mode to MANUAL_MODE when the end is detected
Zachary Sunberg 4:70fea94b29ae 33 // mode = MANUAL_MODE
zsunberg 11:f0df5dea322d 34
zsunberg 11:f0df5dea322d 35 wait_ms(10);
Zachary Sunberg 4:70fea94b29ae 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 12:f96af1d15e9e 45 // g:<p|i|d>:<float>
Zachary Sunberg 12:f96af1d15e9e 46 // change gains
Zachary Sunberg 4:70fea94b29ae 47
Zachary Sunberg 4:70fea94b29ae 48 // OUTPUT MESSAGES
Zachary Sunberg 4:70fea94b29ae 49 // p:<float> line position
Zachary Sunberg 4:70fea94b29ae 50 // s:<int>,<int>,<int>,<int>,<int>
Zachary Sunberg 4:70fea94b29ae 51 // light sensor values
Zachary Sunberg 4:70fea94b29ae 52 // m:<int> mode
Zachary Sunberg 4:70fea94b29ae 53
zsunberg 1:a7aab5937375 54 int parse_command(const char* cmd)
zsunberg 1:a7aab5937375 55 {
zsunberg 1:a7aab5937375 56 if(cmd[1]==':'){
Zachary Sunberg 12:f96af1d15e9e 57 // left motor
Zachary Sunberg 9:7f94942cd50d 58 if(cmd[0]=='l'){
Zachary Sunberg 9:7f94942cd50d 59 if(mode==MANUAL_MODE){
Zachary Sunberg 9:7f94942cd50d 60 leftspeed = atof(&cmd[2]);
Zachary Sunberg 9:7f94942cd50d 61 }
Zachary Sunberg 12:f96af1d15e9e 62 // right motor
Zachary Sunberg 9:7f94942cd50d 63 }else if(cmd[0]=='r'){
Zachary Sunberg 9:7f94942cd50d 64 if(mode==MANUAL_MODE){
Zachary Sunberg 9:7f94942cd50d 65 rightspeed = atof(&cmd[2]);
Zachary Sunberg 9:7f94942cd50d 66 }
Zachary Sunberg 12:f96af1d15e9e 67 // mode
Zachary Sunberg 4:70fea94b29ae 68 }else if(cmd[0]=='c'){
Zachary Sunberg 4:70fea94b29ae 69 mode = atoi(&cmd[2]);
Zachary Sunberg 12:f96af1d15e9e 70 // gains
Zachary Sunberg 12:f96af1d15e9e 71 }else if(cmd[0]=='g'){
Zachary Sunberg 12:f96af1d15e9e 72 if(cmd[2]=='p'){
Zachary Sunberg 12:f96af1d15e9e 73 k_p = atof(&cmd[4]);
Zachary Sunberg 12:f96af1d15e9e 74 }else if(cmd[2]=='i'){
Zachary Sunberg 12:f96af1d15e9e 75 k_i = atof(&cmd[4]);
Zachary Sunberg 12:f96af1d15e9e 76 }else if(cmd[2]=='d'){
Zachary Sunberg 12:f96af1d15e9e 77 k_d = atof(&cmd[4]);
Zachary Sunberg 12:f96af1d15e9e 78 }
Zachary Sunberg 15:0e3360eb84cc 79 xbee.printf("gains p:%f, i:%f, d:%f", k_p, k_i, k_d);
zsunberg 1:a7aab5937375 80 }else{
zsunberg 1:a7aab5937375 81 xbee.printf("%s\n",cmd);
zsunberg 1:a7aab5937375 82 }
zsunberg 1:a7aab5937375 83 }else{
zsunberg 1:a7aab5937375 84 xbee.printf("%s\n",cmd);
zsunberg 1:a7aab5937375 85 }
zsunberg 1:a7aab5937375 86 return 0;
zsunberg 1:a7aab5937375 87 }
zsunberg 0:9f058fe8cab5 88
zsunberg 0:9f058fe8cab5 89 void Rx_interrupt()
zsunberg 0:9f058fe8cab5 90 {
zsunberg 1:a7aab5937375 91 // assume recursive interrupt is not possible
zsunberg 0:9f058fe8cab5 92 led2 = 1;
zsunberg 1:a7aab5937375 93 char read;
zsunberg 0:9f058fe8cab5 94
zsunberg 0:9f058fe8cab5 95 while(xbee.readable()){
zsunberg 1:a7aab5937375 96 read = xbee.getc();
zsunberg 1:a7aab5937375 97 if(read=='\n'){
zsunberg 1:a7aab5937375 98 received[r_index]='\0'; // put a null character at the end
zsunberg 1:a7aab5937375 99 parse_command(received);
zsunberg 1:a7aab5937375 100 r_index=0;
zsunberg 0:9f058fe8cab5 101 } else {
zsunberg 1:a7aab5937375 102 if(r_index >= 80){
zsunberg 1:a7aab5937375 103 r_index=0;
zsunberg 1:a7aab5937375 104 }
zsunberg 1:a7aab5937375 105 received[r_index]=read;
zsunberg 1:a7aab5937375 106 r_index++;
zsunberg 0:9f058fe8cab5 107 }
zsunberg 0:9f058fe8cab5 108 }
zsunberg 0:9f058fe8cab5 109
zsunberg 1:a7aab5937375 110 led2=0;
zsunberg 1:a7aab5937375 111
zsunberg 0:9f058fe8cab5 112 return;
zsunberg 0:9f058fe8cab5 113 }
zsunberg 0:9f058fe8cab5 114
zsunberg 0:9f058fe8cab5 115 int main() {
zsunberg 0:9f058fe8cab5 116
zsunberg 0:9f058fe8cab5 117 xbee.attach(&Rx_interrupt, Serial::RxIrq);
zsunberg 0:9f058fe8cab5 118 xbeeReset = 0;
zsunberg 0:9f058fe8cab5 119 wait(2);
zsunberg 0:9f058fe8cab5 120 xbeeReset = 1;
zsunberg 1:a7aab5937375 121 pi.sensor_auto_calibrate();
zsunberg 1:a7aab5937375 122 r_index = 0;
zsunberg 1:a7aab5937375 123 received[0] = '\0';
zsunberg 7:860856cead84 124 mode = MANUAL_MODE;
zsunberg 0:9f058fe8cab5 125
zsunberg 0:9f058fe8cab5 126 while(1){
Zachary Sunberg 4:70fea94b29ae 127
zsunberg 0:9f058fe8cab5 128 led1 = 1;
Zachary Sunberg 3:47cd6b9e4dbb 129 wait_ms(25);
Zachary Sunberg 4:70fea94b29ae 130 if(mode==LINE_FOLLOW_MODE){
zsunberg 6:fea727557ad5 131 line_follow_loop();
Zachary Sunberg 4:70fea94b29ae 132 }
Zachary Sunberg 8:9792935abfc3 133
Zachary Sunberg 8:9792935abfc3 134 // reversing these is more intuitive
Zachary Sunberg 8:9792935abfc3 135 // pi.left_motor(leftspeed);
Zachary Sunberg 8:9792935abfc3 136 // pi.right_motor(rightspeed);
Zachary Sunberg 9:7f94942cd50d 137 __disable_irq();
Zachary Sunberg 8:9792935abfc3 138 pi.left_motor(rightspeed);
Zachary Sunberg 8:9792935abfc3 139 pi.right_motor(leftspeed);
Zachary Sunberg 9:7f94942cd50d 140 __enable_irq();
Zachary Sunberg 4:70fea94b29ae 141
zsunberg 0:9f058fe8cab5 142 led1 = 0;
Zachary Sunberg 3:47cd6b9e4dbb 143 wait_ms(25);
zsunberg 1:a7aab5937375 144 pi.sensor_reading(sensors);
zsunberg 1:a7aab5937375 145 int* s = sensors;
zsunberg 1:a7aab5937375 146 __disable_irq();
zsunberg 1:a7aab5937375 147 xbee.printf("s:%i,%i,%i,%i,%i\n", s[0], s[1], s[2], s[3], s[4]);
zsunberg 1:a7aab5937375 148 __enable_irq();
zsunberg 1:a7aab5937375 149 __disable_irq();
zsunberg 1:a7aab5937375 150 xbee.printf("p:%f\n", pi.line_position());
zsunberg 1:a7aab5937375 151 __enable_irq();
Zachary Sunberg 4:70fea94b29ae 152 __disable_irq();
Zachary Sunberg 4:70fea94b29ae 153 xbee.printf("m:%d\n", mode);
Zachary Sunberg 4:70fea94b29ae 154 __enable_irq();
zsunberg 0:9f058fe8cab5 155 }
zsunberg 0:9f058fe8cab5 156
zsunberg 0:9f058fe8cab5 157 return 0;
zsunberg 0:9f058fe8cab5 158 }