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.
main.cpp@8:f96af1d15e9e, 2015-07-21 (annotated)
- Committer:
- Zachary Sunberg
- Date:
- Tue Jul 21 15:41:11 2015 -0700
- Revision:
- 8:f96af1d15e9e
- Parent:
- 7:7f94942cd50d
- Child:
- 13:b4bc02a700bb
online updating of gains
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
zsunberg | 0:9f058fe8cab5 | 1 | #include "robot.h" |
zsunberg | 1:a7aab5937375 | 2 | #include <sstream> |
zsunberg | 1:a7aab5937375 | 3 | |
Zachary Sunberg |
8: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 |
8:f96af1d15e9e | 15 | // GAINS |
Zachary Sunberg |
4:70fea94b29ae | 16 | ////////////////////////////// |
Zachary Sunberg |
8:f96af1d15e9e | 17 | float k_p = 0.0; |
Zachary Sunberg |
8:f96af1d15e9e | 18 | float k_i = 0.0; |
Zachary Sunberg |
8: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 |
Zachary Sunberg |
4:70fea94b29ae | 34 | |
Zachary Sunberg |
4:70fea94b29ae | 35 | led4 = 0; |
Zachary Sunberg |
4:70fea94b29ae | 36 | } |
Zachary Sunberg |
4:70fea94b29ae | 37 | |
Zachary Sunberg |
4:70fea94b29ae | 38 | |
Zachary Sunberg |
4:70fea94b29ae | 39 | // INPUT COMMANDS |
Zachary Sunberg |
4:70fea94b29ae | 40 | // l:<float> set left wheel speed (only effective in MANUAL_MODE) |
Zachary Sunberg |
4:70fea94b29ae | 41 | // r:<float> set right wheel speed (only effective in MANUAL_MODE) |
Zachary Sunberg |
4:70fea94b29ae | 42 | // c:<int> change mode |
Zachary Sunberg |
8:f96af1d15e9e | 43 | // g:<p|i|d>:<float> |
Zachary Sunberg |
8:f96af1d15e9e | 44 | // change gains |
Zachary Sunberg |
4:70fea94b29ae | 45 | |
Zachary Sunberg |
4:70fea94b29ae | 46 | // OUTPUT MESSAGES |
Zachary Sunberg |
4:70fea94b29ae | 47 | // p:<float> line position |
Zachary Sunberg |
4:70fea94b29ae | 48 | // s:<int>,<int>,<int>,<int>,<int> |
Zachary Sunberg |
4:70fea94b29ae | 49 | // light sensor values |
Zachary Sunberg |
4:70fea94b29ae | 50 | // m:<int> mode |
Zachary Sunberg |
4:70fea94b29ae | 51 | |
zsunberg | 1:a7aab5937375 | 52 | int parse_command(const char* cmd) |
zsunberg | 1:a7aab5937375 | 53 | { |
zsunberg | 1:a7aab5937375 | 54 | if(cmd[1]==':'){ |
Zachary Sunberg |
8:f96af1d15e9e | 55 | // left motor |
Zachary Sunberg |
7:7f94942cd50d | 56 | if(cmd[0]=='l'){ |
Zachary Sunberg |
7:7f94942cd50d | 57 | if(mode==MANUAL_MODE){ |
Zachary Sunberg |
7:7f94942cd50d | 58 | leftspeed = atof(&cmd[2]); |
Zachary Sunberg |
7:7f94942cd50d | 59 | } |
Zachary Sunberg |
8:f96af1d15e9e | 60 | // right motor |
Zachary Sunberg |
7:7f94942cd50d | 61 | }else if(cmd[0]=='r'){ |
Zachary Sunberg |
7:7f94942cd50d | 62 | if(mode==MANUAL_MODE){ |
Zachary Sunberg |
7:7f94942cd50d | 63 | rightspeed = atof(&cmd[2]); |
Zachary Sunberg |
7:7f94942cd50d | 64 | } |
Zachary Sunberg |
8:f96af1d15e9e | 65 | // mode |
Zachary Sunberg |
4:70fea94b29ae | 66 | }else if(cmd[0]=='c'){ |
Zachary Sunberg |
4:70fea94b29ae | 67 | mode = atoi(&cmd[2]); |
Zachary Sunberg |
8:f96af1d15e9e | 68 | // gains |
Zachary Sunberg |
8:f96af1d15e9e | 69 | }else if(cmd[0]=='g'){ |
Zachary Sunberg |
8:f96af1d15e9e | 70 | if(cmd[2]=='p'){ |
Zachary Sunberg |
8:f96af1d15e9e | 71 | k_p = atof(&cmd[4]); |
Zachary Sunberg |
8:f96af1d15e9e | 72 | }else if(cmd[2]=='i'){ |
Zachary Sunberg |
8:f96af1d15e9e | 73 | k_i = atof(&cmd[4]); |
Zachary Sunberg |
8:f96af1d15e9e | 74 | }else if(cmd[2]=='d'){ |
Zachary Sunberg |
8:f96af1d15e9e | 75 | k_d = atof(&cmd[4]); |
Zachary Sunberg |
8:f96af1d15e9e | 76 | } |
zsunberg | 1:a7aab5937375 | 77 | }else{ |
zsunberg | 1:a7aab5937375 | 78 | xbee.printf("%s\n",cmd); |
zsunberg | 1:a7aab5937375 | 79 | } |
zsunberg | 1:a7aab5937375 | 80 | }else{ |
zsunberg | 1:a7aab5937375 | 81 | xbee.printf("%s\n",cmd); |
zsunberg | 1:a7aab5937375 | 82 | } |
zsunberg | 1:a7aab5937375 | 83 | return 0; |
zsunberg | 1:a7aab5937375 | 84 | } |
zsunberg | 0:9f058fe8cab5 | 85 | |
zsunberg | 0:9f058fe8cab5 | 86 | void Rx_interrupt() |
zsunberg | 0:9f058fe8cab5 | 87 | { |
zsunberg | 1:a7aab5937375 | 88 | // assume recursive interrupt is not possible |
zsunberg | 0:9f058fe8cab5 | 89 | led2 = 1; |
zsunberg | 1:a7aab5937375 | 90 | char read; |
zsunberg | 0:9f058fe8cab5 | 91 | |
zsunberg | 0:9f058fe8cab5 | 92 | while(xbee.readable()){ |
zsunberg | 1:a7aab5937375 | 93 | read = xbee.getc(); |
zsunberg | 1:a7aab5937375 | 94 | if(read=='\n'){ |
zsunberg | 1:a7aab5937375 | 95 | received[r_index]='\0'; // put a null character at the end |
zsunberg | 1:a7aab5937375 | 96 | parse_command(received); |
zsunberg | 1:a7aab5937375 | 97 | r_index=0; |
zsunberg | 0:9f058fe8cab5 | 98 | } else { |
zsunberg | 1:a7aab5937375 | 99 | if(r_index >= 80){ |
zsunberg | 1:a7aab5937375 | 100 | r_index=0; |
zsunberg | 1:a7aab5937375 | 101 | } |
zsunberg | 1:a7aab5937375 | 102 | received[r_index]=read; |
zsunberg | 1:a7aab5937375 | 103 | r_index++; |
zsunberg | 0:9f058fe8cab5 | 104 | } |
zsunberg | 0:9f058fe8cab5 | 105 | } |
zsunberg | 0:9f058fe8cab5 | 106 | |
zsunberg | 1:a7aab5937375 | 107 | led2=0; |
zsunberg | 1:a7aab5937375 | 108 | |
zsunberg | 0:9f058fe8cab5 | 109 | return; |
zsunberg | 0:9f058fe8cab5 | 110 | } |
zsunberg | 0:9f058fe8cab5 | 111 | |
zsunberg | 0:9f058fe8cab5 | 112 | int main() { |
zsunberg | 0:9f058fe8cab5 | 113 | |
zsunberg | 0:9f058fe8cab5 | 114 | xbee.attach(&Rx_interrupt, Serial::RxIrq); |
zsunberg | 0:9f058fe8cab5 | 115 | xbeeReset = 0; |
zsunberg | 0:9f058fe8cab5 | 116 | wait(2); |
zsunberg | 0:9f058fe8cab5 | 117 | xbeeReset = 1; |
zsunberg | 1:a7aab5937375 | 118 | pi.sensor_auto_calibrate(); |
zsunberg | 1:a7aab5937375 | 119 | r_index = 0; |
zsunberg | 1:a7aab5937375 | 120 | received[0] = '\0'; |
zsunberg | 0:9f058fe8cab5 | 121 | |
zsunberg | 0:9f058fe8cab5 | 122 | while(1){ |
Zachary Sunberg |
4:70fea94b29ae | 123 | |
zsunberg | 0:9f058fe8cab5 | 124 | led1 = 1; |
Zachary Sunberg |
3:47cd6b9e4dbb | 125 | wait_ms(25); |
Zachary Sunberg |
4:70fea94b29ae | 126 | if(mode==LINE_FOLLOW_MODE){ |
Zachary Sunberg |
4:70fea94b29ae | 127 | line_follow_loop() |
Zachary Sunberg |
4:70fea94b29ae | 128 | } |
Zachary Sunberg |
6:9792935abfc3 | 129 | |
Zachary Sunberg |
6:9792935abfc3 | 130 | // reversing these is more intuitive |
Zachary Sunberg |
6:9792935abfc3 | 131 | // pi.left_motor(leftspeed); |
Zachary Sunberg |
6:9792935abfc3 | 132 | // pi.right_motor(rightspeed); |
Zachary Sunberg |
7:7f94942cd50d | 133 | __disable_irq(); |
Zachary Sunberg |
6:9792935abfc3 | 134 | pi.left_motor(rightspeed); |
Zachary Sunberg |
6:9792935abfc3 | 135 | pi.right_motor(leftspeed); |
Zachary Sunberg |
7:7f94942cd50d | 136 | __enable_irq(); |
Zachary Sunberg |
4:70fea94b29ae | 137 | |
zsunberg | 0:9f058fe8cab5 | 138 | led1 = 0; |
Zachary Sunberg |
3:47cd6b9e4dbb | 139 | wait_ms(25); |
zsunberg | 1:a7aab5937375 | 140 | pi.sensor_reading(sensors); |
zsunberg | 1:a7aab5937375 | 141 | int* s = sensors; |
zsunberg | 1:a7aab5937375 | 142 | __disable_irq(); |
zsunberg | 1:a7aab5937375 | 143 | xbee.printf("s:%i,%i,%i,%i,%i\n", s[0], s[1], s[2], s[3], s[4]); |
zsunberg | 1:a7aab5937375 | 144 | __enable_irq(); |
zsunberg | 1:a7aab5937375 | 145 | __disable_irq(); |
zsunberg | 1:a7aab5937375 | 146 | xbee.printf("p:%f\n", pi.line_position()); |
zsunberg | 1:a7aab5937375 | 147 | __enable_irq(); |
Zachary Sunberg |
4:70fea94b29ae | 148 | __disable_irq(); |
Zachary Sunberg |
4:70fea94b29ae | 149 | xbee.printf("m:%d\n", mode); |
Zachary Sunberg |
4:70fea94b29ae | 150 | __enable_irq(); |
zsunberg | 0:9f058fe8cab5 | 151 | } |
zsunberg | 0:9f058fe8cab5 | 152 | |
zsunberg | 0:9f058fe8cab5 | 153 | return 0; |
zsunberg | 0:9f058fe8cab5 | 154 | } |