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@4:70fea94b29ae, 2015-07-21 (annotated)

Committer:
Zachary Sunberg
Date:
Tue Jul 21 14:39:19 2015 -0700
Revision:
4:70fea94b29ae
Parent:
3:47cd6b9e4dbb
Child:
5:70c86dbc8832
added modes

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 4:70fea94b29ae 4 int mode;
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 4:70fea94b29ae 15 // PUT GLOBAL CONSTANTS HERE
Zachary Sunberg 4:70fea94b29ae 16 //////////////////////////////
Zachary Sunberg 4:70fea94b29ae 17
Zachary Sunberg 4:70fea94b29ae 18 //////////////////////////////
zsunberg 0:9f058fe8cab5 19
zsunberg 1:a7aab5937375 20 /*
Zachary Sunberg 4:70fea94b29ae 21 * This function will be called at approximately 20 Hz when the control mode is LINE_FOLLOW_MODE
Zachary Sunberg 4:70fea94b29ae 22 */
Zachary Sunberg 4:70fea94b29ae 23 int line_follow_loop(){
Zachary Sunberg 4:70fea94b29ae 24 led4 = 1;
Zachary Sunberg 4:70fea94b29ae 25
Zachary Sunberg 4:70fea94b29ae 26 // set these variables to control the robot
Zachary Sunberg 4:70fea94b29ae 27 leftspeed = 0.0;
Zachary Sunberg 4:70fea94b29ae 28 rightspeed = 0.0;
Zachary Sunberg 4:70fea94b29ae 29
Zachary Sunberg 4:70fea94b29ae 30 // set mode to MANUAL_MODE when the end is detected
Zachary Sunberg 4:70fea94b29ae 31 // mode = MANUAL_MODE
Zachary Sunberg 4:70fea94b29ae 32
Zachary Sunberg 4:70fea94b29ae 33 led4 = 0;
Zachary Sunberg 4:70fea94b29ae 34 }
Zachary Sunberg 4:70fea94b29ae 35
Zachary Sunberg 4:70fea94b29ae 36
Zachary Sunberg 4:70fea94b29ae 37 // INPUT COMMANDS
Zachary Sunberg 4:70fea94b29ae 38 // l:<float> set left wheel speed (only effective in MANUAL_MODE)
Zachary Sunberg 4:70fea94b29ae 39 // r:<float> set right wheel speed (only effective in MANUAL_MODE)
Zachary Sunberg 4:70fea94b29ae 40 // c:<int> change mode
Zachary Sunberg 4:70fea94b29ae 41
Zachary Sunberg 4:70fea94b29ae 42 // OUTPUT MESSAGES
Zachary Sunberg 4:70fea94b29ae 43 // p:<float> line position
Zachary Sunberg 4:70fea94b29ae 44 // s:<int>,<int>,<int>,<int>,<int>
Zachary Sunberg 4:70fea94b29ae 45 // light sensor values
Zachary Sunberg 4:70fea94b29ae 46 // m:<int> mode
Zachary Sunberg 4:70fea94b29ae 47
zsunberg 1:a7aab5937375 48 int parse_command(const char* cmd)
zsunberg 1:a7aab5937375 49 {
zsunberg 1:a7aab5937375 50 if(cmd[1]==':'){
Zachary Sunberg 4:70fea94b29ae 51 if(cmd[0]=='l' && mode==MANUAL_MODE){
zsunberg 1:a7aab5937375 52 leftspeed = atof(&cmd[2]);
Zachary Sunberg 4:70fea94b29ae 53 }else if(cmd[0]=='r' && mode==MANUAL_MODE){
zsunberg 1:a7aab5937375 54 rightspeed = atof(&cmd[2]);
Zachary Sunberg 4:70fea94b29ae 55 }else if(cmd[0]=='c'){
Zachary Sunberg 4:70fea94b29ae 56 mode = atoi(&cmd[2]);
Zachary Sunberg 4:70fea94b29ae 57 }
zsunberg 1:a7aab5937375 58 }else{
zsunberg 1:a7aab5937375 59 xbee.printf("%s\n",cmd);
zsunberg 1:a7aab5937375 60 }
zsunberg 1:a7aab5937375 61 }else{
zsunberg 1:a7aab5937375 62 xbee.printf("%s\n",cmd);
zsunberg 1:a7aab5937375 63 }
zsunberg 1:a7aab5937375 64 return 0;
zsunberg 1:a7aab5937375 65 }
zsunberg 0:9f058fe8cab5 66
zsunberg 0:9f058fe8cab5 67 void Rx_interrupt()
zsunberg 0:9f058fe8cab5 68 {
zsunberg 1:a7aab5937375 69 // assume recursive interrupt is not possible
zsunberg 0:9f058fe8cab5 70 led2 = 1;
zsunberg 1:a7aab5937375 71 char read;
zsunberg 0:9f058fe8cab5 72
zsunberg 0:9f058fe8cab5 73 while(xbee.readable()){
zsunberg 1:a7aab5937375 74 read = xbee.getc();
zsunberg 1:a7aab5937375 75 if(read=='\n'){
zsunberg 1:a7aab5937375 76 received[r_index]='\0'; // put a null character at the end
zsunberg 1:a7aab5937375 77 parse_command(received);
zsunberg 1:a7aab5937375 78 r_index=0;
zsunberg 0:9f058fe8cab5 79 } else {
zsunberg 1:a7aab5937375 80 if(r_index >= 80){
zsunberg 1:a7aab5937375 81 r_index=0;
zsunberg 1:a7aab5937375 82 }
zsunberg 1:a7aab5937375 83 received[r_index]=read;
zsunberg 1:a7aab5937375 84 r_index++;
zsunberg 0:9f058fe8cab5 85 }
zsunberg 0:9f058fe8cab5 86 }
zsunberg 0:9f058fe8cab5 87
zsunberg 1:a7aab5937375 88 led2=0;
zsunberg 1:a7aab5937375 89
zsunberg 0:9f058fe8cab5 90 return;
zsunberg 0:9f058fe8cab5 91 }
zsunberg 0:9f058fe8cab5 92
zsunberg 0:9f058fe8cab5 93 int main() {
zsunberg 0:9f058fe8cab5 94
zsunberg 0:9f058fe8cab5 95 xbee.attach(&Rx_interrupt, Serial::RxIrq);
zsunberg 0:9f058fe8cab5 96 xbeeReset = 0;
zsunberg 0:9f058fe8cab5 97 wait(2);
zsunberg 0:9f058fe8cab5 98 xbeeReset = 1;
zsunberg 1:a7aab5937375 99 pi.sensor_auto_calibrate();
zsunberg 1:a7aab5937375 100 r_index = 0;
zsunberg 1:a7aab5937375 101 received[0] = '\0';
zsunberg 0:9f058fe8cab5 102
zsunberg 0:9f058fe8cab5 103 while(1){
Zachary Sunberg 4:70fea94b29ae 104
zsunberg 0:9f058fe8cab5 105 led1 = 1;
Zachary Sunberg 3:47cd6b9e4dbb 106 wait_ms(25);
Zachary Sunberg 4:70fea94b29ae 107 if(mode==LINE_FOLLOW_MODE){
Zachary Sunberg 4:70fea94b29ae 108 line_follow_loop()
Zachary Sunberg 4:70fea94b29ae 109 }
zsunberg 1:a7aab5937375 110 pi.left_motor(leftspeed);
zsunberg 1:a7aab5937375 111 pi.right_motor(rightspeed);
Zachary Sunberg 4:70fea94b29ae 112
zsunberg 0:9f058fe8cab5 113 led1 = 0;
Zachary Sunberg 3:47cd6b9e4dbb 114 wait_ms(25);
zsunberg 1:a7aab5937375 115 pi.sensor_reading(sensors);
zsunberg 1:a7aab5937375 116 int* s = sensors;
zsunberg 1:a7aab5937375 117 __disable_irq();
zsunberg 1:a7aab5937375 118 xbee.printf("s:%i,%i,%i,%i,%i\n", s[0], s[1], s[2], s[3], s[4]);
zsunberg 1:a7aab5937375 119 __enable_irq();
zsunberg 1:a7aab5937375 120 __disable_irq();
zsunberg 1:a7aab5937375 121 xbee.printf("p:%f\n", pi.line_position());
zsunberg 1:a7aab5937375 122 __enable_irq();
Zachary Sunberg 4:70fea94b29ae 123 __disable_irq();
Zachary Sunberg 4:70fea94b29ae 124 xbee.printf("m:%d\n", mode);
Zachary Sunberg 4:70fea94b29ae 125 __enable_irq();
zsunberg 0:9f058fe8cab5 126 }
zsunberg 0:9f058fe8cab5 127
zsunberg 0:9f058fe8cab5 128 return 0;
zsunberg 0:9f058fe8cab5 129 }