![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
main.cpp@45:77cf6375348a, 2013-04-12 (annotated)
- Committer:
- xiaxia686
- Date:
- Fri Apr 12 20:40:52 2013 +0000
- Revision:
- 45:77cf6375348a
- Parent:
- 44:555136de33e4
- Child:
- 46:adcd57a5e402
colour sensors fixed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
madcowswe | 20:70d651156779 | 1 | #include "globals.h" |
madcowswe | 20:70d651156779 | 2 | #include "Kalman.h" |
twighk | 0:200635fa1b08 | 3 | #include "mbed.h" |
twighk | 13:d4b5851742a3 | 4 | #include "rtos.h" |
madcowswe | 21:167dacfe0b14 | 5 | #include "Arm.h" |
madcowswe | 21:167dacfe0b14 | 6 | #include "MainMotor.h" |
madcowswe | 21:167dacfe0b14 | 7 | #include "Encoder.h" |
madcowswe | 21:167dacfe0b14 | 8 | #include "Colour.h" |
madcowswe | 21:167dacfe0b14 | 9 | #include "CakeSensor.h" |
madcowswe | 21:167dacfe0b14 | 10 | #include "Printing.h" |
madcowswe | 20:70d651156779 | 11 | #include "coprocserial.h" |
madcowswe | 12:76c9915db820 | 12 | #include <algorithm> |
rsavitski | 24:50805ef8c499 | 13 | #include "motion.h" |
madcowswe | 25:b16f1045108f | 14 | #include "MotorControl.h" |
madcowswe | 28:4e20b44251c6 | 15 | #include "system.h" |
rsavitski | 30:791739422122 | 16 | #include "ai.h" |
madcowswe | 20:70d651156779 | 17 | |
twighk | 0:200635fa1b08 | 18 | void motortest(); |
twighk | 0:200635fa1b08 | 19 | void encodertest(); |
twighk | 0:200635fa1b08 | 20 | void motorencodetest(); |
twighk | 0:200635fa1b08 | 21 | void motorencodetestline(); |
twighk | 0:200635fa1b08 | 22 | void motorsandservostest(); |
twighk | 1:8119211eae14 | 23 | void armtest(); |
twighk | 2:45da48fab346 | 24 | void motortestline(); |
twighk | 3:717de74f6ebd | 25 | void colourtest(); |
xiaxia686 | 43:c592bf6a6a2d | 26 | void pt_test(); |
twighk | 8:69bdf20cb525 | 27 | void cakesensortest(); |
twighk | 13:d4b5851742a3 | 28 | void printingtestthread(void const*); |
twighk | 13:d4b5851742a3 | 29 | void printingtestthread2(void const*); |
madcowswe | 12:76c9915db820 | 30 | void feedbacktest(); |
twighk | 0:200635fa1b08 | 31 | |
xiaxia686 | 43:c592bf6a6a2d | 32 | // bytes packing for peer to peer communication |
xiaxia686 | 43:c592bf6a6a2d | 33 | typedef union { |
xiaxia686 | 43:c592bf6a6a2d | 34 | struct _data { |
xiaxia686 | 43:c592bf6a6a2d | 35 | unsigned char header[3]; |
xiaxia686 | 43:c592bf6a6a2d | 36 | unsigned char ID; |
xiaxia686 | 43:c592bf6a6a2d | 37 | float value; |
xiaxia686 | 43:c592bf6a6a2d | 38 | float aux; |
xiaxia686 | 43:c592bf6a6a2d | 39 | } data; |
xiaxia686 | 43:c592bf6a6a2d | 40 | unsigned char type_char[sizeof(_data)]; |
xiaxia686 | 43:c592bf6a6a2d | 41 | } bytepack_t; |
xiaxia686 | 43:c592bf6a6a2d | 42 | |
xiaxia686 | 44:555136de33e4 | 43 | Serial pc(USBTX, USBRX); |
xiaxia686 | 44:555136de33e4 | 44 | |
xiaxia686 | 43:c592bf6a6a2d | 45 | int main() |
xiaxia686 | 43:c592bf6a6a2d | 46 | { |
xiaxia686 | 43:c592bf6a6a2d | 47 | |
xiaxia686 | 43:c592bf6a6a2d | 48 | /***************** |
xiaxia686 | 43:c592bf6a6a2d | 49 | * Test Code * |
xiaxia686 | 43:c592bf6a6a2d | 50 | *****************/ |
twighk | 0:200635fa1b08 | 51 | //motortest(); |
twighk | 0:200635fa1b08 | 52 | //encodertest(); |
twighk | 8:69bdf20cb525 | 53 | //motorencodetest(); |
twighk | 1:8119211eae14 | 54 | //motorencodetestline(); |
twighk | 0:200635fa1b08 | 55 | //motorsandservostest(); |
twighk | 3:717de74f6ebd | 56 | //armtest(); |
twighk | 2:45da48fab346 | 57 | //motortestline(); |
twighk | 11:bbddc908c78c | 58 | //ledtest(); |
twighk | 3:717de74f6ebd | 59 | //phototransistortest(); |
madcowswe | 12:76c9915db820 | 60 | //ledphototransistortest(); |
madcowswe | 5:56a5fdd373c9 | 61 | //colourtest(); // Red SnR too low |
madcowswe | 12:76c9915db820 | 62 | //cakesensortest(); |
madcowswe | 20:70d651156779 | 63 | //feedbacktest(); |
xiaxia686 | 43:c592bf6a6a2d | 64 | |
xiaxia686 | 43:c592bf6a6a2d | 65 | /* |
twighk | 13:d4b5851742a3 | 66 | DigitalOut l1(LED1); |
madcowswe | 21:167dacfe0b14 | 67 | Thread p(Printing::printingloop, NULL, osPriorityNormal, 2048); |
twighk | 13:d4b5851742a3 | 68 | l1=1; |
twighk | 13:d4b5851742a3 | 69 | Thread a(printingtestthread, NULL, osPriorityNormal, 1024); |
twighk | 13:d4b5851742a3 | 70 | Thread b(printingtestthread2, NULL, osPriorityNormal, 1024); |
twighk | 13:d4b5851742a3 | 71 | Thread::wait(osWaitForever); |
madcowswe | 15:9c5aaeda36dc | 72 | */ |
madcowswe | 16:52250d8d8fce | 73 | |
madcowswe | 22:6e3218cf75f8 | 74 | SystemTime.start(); |
madcowswe | 20:70d651156779 | 75 | |
xiaxia686 | 44:555136de33e4 | 76 | |
madcowswe | 25:b16f1045108f | 77 | pc.baud(115200); |
madcowswe | 25:b16f1045108f | 78 | |
xiaxia686 | 45:77cf6375348a | 79 | // InitSerial(); |
xiaxia686 | 45:77cf6375348a | 80 | // wait(1); |
xiaxia686 | 45:77cf6375348a | 81 | // Kalman::KalmanInit(); |
madcowswe | 20:70d651156779 | 82 | |
xiaxia686 | 45:77cf6375348a | 83 | // Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k |
xiaxia686 | 45:77cf6375348a | 84 | // Kalman::start_predict_ticker(&predictthread); |
madcowswe | 20:70d651156779 | 85 | |
xiaxia686 | 45:77cf6375348a | 86 | // Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084); |
rsavitski | 24:50805ef8c499 | 87 | |
xiaxia686 | 45:77cf6375348a | 88 | // Ticker motorcontroltestticker; |
xiaxia686 | 45:77cf6375348a | 89 | // motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05); |
rsavitski | 30:791739422122 | 90 | // ai layer thread |
xiaxia686 | 45:77cf6375348a | 91 | // Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048); |
madcowswe | 22:6e3218cf75f8 | 92 | |
rsavitski | 24:50805ef8c499 | 93 | // motion layer periodic callback |
xiaxia686 | 45:77cf6375348a | 94 | // RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic); |
xiaxia686 | 45:77cf6375348a | 95 | // motion_timer.start(50); |
xiaxia686 | 43:c592bf6a6a2d | 96 | |
xiaxia686 | 45:77cf6375348a | 97 | // Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048); |
xiaxia686 | 43:c592bf6a6a2d | 98 | |
xiaxia686 | 45:77cf6375348a | 99 | // measureCPUidle(); //repurpose thread for idle measurement |
xiaxia686 | 45:77cf6375348a | 100 | colourtest(); |
xiaxia686 | 43:c592bf6a6a2d | 101 | //pt_test(); |
xiaxia686 | 43:c592bf6a6a2d | 102 | while(true) { |
madcowswe | 21:167dacfe0b14 | 103 | Thread::wait(osWaitForever); |
xiaxia686 | 43:c592bf6a6a2d | 104 | } |
madcowswe | 12:76c9915db820 | 105 | } |
madcowswe | 12:76c9915db820 | 106 | |
twighk | 13:d4b5851742a3 | 107 | #include <cstdlib> |
twighk | 13:d4b5851742a3 | 108 | using namespace std; |
twighk | 13:d4b5851742a3 | 109 | |
xiaxia686 | 43:c592bf6a6a2d | 110 | void printingtestthread(void const*) |
xiaxia686 | 43:c592bf6a6a2d | 111 | { |
twighk | 13:d4b5851742a3 | 112 | const char ID = 1; |
twighk | 13:d4b5851742a3 | 113 | float buffer[3] = {ID}; |
madcowswe | 21:167dacfe0b14 | 114 | Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0])); |
twighk | 13:d4b5851742a3 | 115 | while (true){ |
twighk | 13:d4b5851742a3 | 116 | for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){ |
madcowswe | 20:70d651156779 | 117 | buffer[i] = ID ; |
twighk | 13:d4b5851742a3 | 118 | } |
madcowswe | 21:167dacfe0b14 | 119 | Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0])); |
twighk | 13:d4b5851742a3 | 120 | Thread::wait(200); |
twighk | 13:d4b5851742a3 | 121 | } |
twighk | 13:d4b5851742a3 | 122 | } |
madcowswe | 14:c638d4b9ee94 | 123 | |
xiaxia686 | 43:c592bf6a6a2d | 124 | void printingtestthread2(void const*) |
xiaxia686 | 43:c592bf6a6a2d | 125 | { |
twighk | 13:d4b5851742a3 | 126 | const char ID = 2; |
twighk | 13:d4b5851742a3 | 127 | float buffer[5] = {ID}; |
madcowswe | 21:167dacfe0b14 | 128 | Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0])); |
twighk | 13:d4b5851742a3 | 129 | while (true){ |
twighk | 13:d4b5851742a3 | 130 | for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){ |
twighk | 13:d4b5851742a3 | 131 | buffer[i] = ID; |
twighk | 13:d4b5851742a3 | 132 | } |
madcowswe | 21:167dacfe0b14 | 133 | Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0])); |
twighk | 13:d4b5851742a3 | 134 | Thread::wait(500); |
twighk | 13:d4b5851742a3 | 135 | } |
twighk | 8:69bdf20cb525 | 136 | } |
twighk | 8:69bdf20cb525 | 137 | |
madcowswe | 14:c638d4b9ee94 | 138 | |
rsavitski | 24:50805ef8c499 | 139 | /* |
madcowswe | 12:76c9915db820 | 140 | void feedbacktest(){ |
madcowswe | 20:70d651156779 | 141 | //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B); |
madcowswe | 12:76c9915db820 | 142 | MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); |
xiaxia686 | 43:c592bf6a6a2d | 143 | |
madcowswe | 20:70d651156779 | 144 | Kalman::State state; |
xiaxia686 | 43:c592bf6a6a2d | 145 | |
madcowswe | 20:70d651156779 | 146 | float Pgain = -0.01; |
madcowswe | 15:9c5aaeda36dc | 147 | float fwdspeed = -400/3.0f; |
madcowswe | 12:76c9915db820 | 148 | Timer timer; |
madcowswe | 12:76c9915db820 | 149 | timer.start(); |
xiaxia686 | 43:c592bf6a6a2d | 150 | |
xiaxia686 | 43:c592bf6a6a2d | 151 | while(true) { |
madcowswe | 12:76c9915db820 | 152 | float expecdist = fwdspeed * timer.read(); |
madcowswe | 20:70d651156779 | 153 | state = Kalman::getState(); |
madcowswe | 20:70d651156779 | 154 | float errleft = left_encoder.getTicks() - (expecdist); |
madcowswe | 20:70d651156779 | 155 | float errright = right_encoder.getTicks() - expecdist; |
xiaxia686 | 43:c592bf6a6a2d | 156 | |
madcowswe | 12:76c9915db820 | 157 | mleft(max(min(errleft*Pgain, 0.4f), -0.4f)); |
madcowswe | 12:76c9915db820 | 158 | mright(max(min(errright*Pgain, 0.4f), -0.4f)); |
madcowswe | 12:76c9915db820 | 159 | } |
twighk | 8:69bdf20cb525 | 160 | } |
rsavitski | 24:50805ef8c499 | 161 | */ |
twighk | 8:69bdf20cb525 | 162 | |
xiaxia686 | 43:c592bf6a6a2d | 163 | void cakesensortest() |
xiaxia686 | 43:c592bf6a6a2d | 164 | { |
twighk | 8:69bdf20cb525 | 165 | wait(1); |
madcowswe | 20:70d651156779 | 166 | printf("cakesensortest"); |
xiaxia686 | 43:c592bf6a6a2d | 167 | |
xiaxia686 | 43:c592bf6a6a2d | 168 | CakeSensor cs(P_DISTANCE_SENSOR); |
xiaxia686 | 43:c592bf6a6a2d | 169 | while(true) { |
twighk | 8:69bdf20cb525 | 170 | wait(0.1); |
madcowswe | 20:70d651156779 | 171 | printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm()); |
xiaxia686 | 43:c592bf6a6a2d | 172 | } |
twighk | 3:717de74f6ebd | 173 | } |
twighk | 3:717de74f6ebd | 174 | |
xiaxia686 | 43:c592bf6a6a2d | 175 | void colourtest() |
xiaxia686 | 43:c592bf6a6a2d | 176 | { |
xiaxia686 | 45:77cf6375348a | 177 | Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER); |
xiaxia686 | 45:77cf6375348a | 178 | Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER); |
xiaxia686 | 43:c592bf6a6a2d | 179 | |
xiaxia686 | 43:c592bf6a6a2d | 180 | while(true) { |
twighk | 3:717de74f6ebd | 181 | wait(0.1); |
xiaxia686 | 43:c592bf6a6a2d | 182 | |
xiaxia686 | 45:77cf6375348a | 183 | switch(c_lower.getColour()) { |
twighk | 3:717de74f6ebd | 184 | case BLUE : |
xiaxia686 | 43:c592bf6a6a2d | 185 | printf("BLUE\n"); |
twighk | 3:717de74f6ebd | 186 | break; |
twighk | 3:717de74f6ebd | 187 | case RED: |
xiaxia686 | 43:c592bf6a6a2d | 188 | printf("RED\n"); |
twighk | 3:717de74f6ebd | 189 | break; |
twighk | 3:717de74f6ebd | 190 | case WHITE: |
xiaxia686 | 43:c592bf6a6a2d | 191 | printf("WHITE\n"); |
twighk | 3:717de74f6ebd | 192 | break; |
xiaxia686 | 43:c592bf6a6a2d | 193 | case BLACK: |
xiaxia686 | 43:c592bf6a6a2d | 194 | printf("BLACK\n"); |
twighk | 3:717de74f6ebd | 195 | break; |
twighk | 3:717de74f6ebd | 196 | default: |
xiaxia686 | 43:c592bf6a6a2d | 197 | printf("BUG\n"); |
twighk | 3:717de74f6ebd | 198 | } |
xiaxia686 | 43:c592bf6a6a2d | 199 | |
twighk | 2:45da48fab346 | 200 | } |
twighk | 0:200635fa1b08 | 201 | |
twighk | 3:717de74f6ebd | 202 | } |
twighk | 3:717de74f6ebd | 203 | |
xiaxia686 | 43:c592bf6a6a2d | 204 | void pt_test() |
xiaxia686 | 43:c592bf6a6a2d | 205 | { |
xiaxia686 | 43:c592bf6a6a2d | 206 | DigitalOut _blue_led(p10); |
xiaxia686 | 43:c592bf6a6a2d | 207 | DigitalOut _red_led(p11); |
xiaxia686 | 45:77cf6375348a | 208 | AnalogIn _pt(p18); |
xiaxia686 | 43:c592bf6a6a2d | 209 | |
xiaxia686 | 43:c592bf6a6a2d | 210 | bytepack_t datapackage; |
xiaxia686 | 43:c592bf6a6a2d | 211 | // first 3 bytes of header is used for alignment |
xiaxia686 | 43:c592bf6a6a2d | 212 | datapackage.data.header[0] = 0xFF; |
xiaxia686 | 43:c592bf6a6a2d | 213 | datapackage.data.header[1] = 0xFF; |
xiaxia686 | 43:c592bf6a6a2d | 214 | datapackage.data.header[2] = 0xFF; |
xiaxia686 | 43:c592bf6a6a2d | 215 | while(true) { |
xiaxia686 | 43:c592bf6a6a2d | 216 | //toggles leds for the next state |
xiaxia686 | 43:c592bf6a6a2d | 217 | _blue_led = 1; |
xiaxia686 | 43:c592bf6a6a2d | 218 | _red_led = 0; |
xiaxia686 | 43:c592bf6a6a2d | 219 | wait(0.01); |
xiaxia686 | 43:c592bf6a6a2d | 220 | volatile float blue_temp = _pt.read(); |
twighk | 3:717de74f6ebd | 221 | |
xiaxia686 | 43:c592bf6a6a2d | 222 | |
xiaxia686 | 43:c592bf6a6a2d | 223 | datapackage.data.ID = (unsigned char)0; |
xiaxia686 | 43:c592bf6a6a2d | 224 | datapackage.data.value = blue_temp; |
xiaxia686 | 43:c592bf6a6a2d | 225 | datapackage.data.aux = 0; |
xiaxia686 | 43:c592bf6a6a2d | 226 | for (int i = 0; i < sizeof(datapackage); i++) { |
xiaxia686 | 43:c592bf6a6a2d | 227 | //mbed_serial.putc(datapackage.type_char[i]); |
xiaxia686 | 43:c592bf6a6a2d | 228 | pc.putc(datapackage.type_char[i]); |
twighk | 3:717de74f6ebd | 229 | } |
xiaxia686 | 43:c592bf6a6a2d | 230 | |
xiaxia686 | 43:c592bf6a6a2d | 231 | _blue_led = 0; |
xiaxia686 | 43:c592bf6a6a2d | 232 | _red_led = 1; |
xiaxia686 | 43:c592bf6a6a2d | 233 | wait(0.01); |
xiaxia686 | 43:c592bf6a6a2d | 234 | volatile float red_temp = _pt.read(); |
xiaxia686 | 43:c592bf6a6a2d | 235 | |
xiaxia686 | 43:c592bf6a6a2d | 236 | |
xiaxia686 | 43:c592bf6a6a2d | 237 | datapackage.data.ID = (unsigned char)1; |
xiaxia686 | 43:c592bf6a6a2d | 238 | datapackage.data.value = red_temp; |
xiaxia686 | 43:c592bf6a6a2d | 239 | datapackage.data.aux = 0; |
xiaxia686 | 43:c592bf6a6a2d | 240 | for (int i = 0; i < sizeof(datapackage); i++) { |
xiaxia686 | 43:c592bf6a6a2d | 241 | //mbed_serial.putc(datapackage.type_char[i]); |
xiaxia686 | 43:c592bf6a6a2d | 242 | pc.putc(datapackage.type_char[i]); |
twighk | 11:bbddc908c78c | 243 | } |
twighk | 3:717de74f6ebd | 244 | |
xiaxia686 | 43:c592bf6a6a2d | 245 | _blue_led = 0; |
xiaxia686 | 43:c592bf6a6a2d | 246 | _red_led = 0; |
xiaxia686 | 43:c592bf6a6a2d | 247 | wait(0.01); |
xiaxia686 | 43:c592bf6a6a2d | 248 | volatile float noise_temp = _pt.read(); |
twighk | 3:717de74f6ebd | 249 | |
twighk | 3:717de74f6ebd | 250 | |
xiaxia686 | 43:c592bf6a6a2d | 251 | datapackage.data.ID = (unsigned char)2; |
xiaxia686 | 43:c592bf6a6a2d | 252 | datapackage.data.value = noise_temp; |
xiaxia686 | 43:c592bf6a6a2d | 253 | datapackage.data.aux = 0; |
xiaxia686 | 43:c592bf6a6a2d | 254 | for (int i = 0; i < sizeof(datapackage); i++) { |
xiaxia686 | 43:c592bf6a6a2d | 255 | //mbed_serial.putc(datapackage.type_char[i]); |
xiaxia686 | 43:c592bf6a6a2d | 256 | pc.putc(datapackage.type_char[i]); |
xiaxia686 | 43:c592bf6a6a2d | 257 | } |
xiaxia686 | 43:c592bf6a6a2d | 258 | |
twighk | 3:717de74f6ebd | 259 | } |
twighk | 3:717de74f6ebd | 260 | } |
twighk | 3:717de74f6ebd | 261 | |
xiaxia686 | 43:c592bf6a6a2d | 262 | |
xiaxia686 | 43:c592bf6a6a2d | 263 | |
xiaxia686 | 43:c592bf6a6a2d | 264 | void armtest() |
xiaxia686 | 43:c592bf6a6a2d | 265 | { |
twighk | 3:717de74f6ebd | 266 | Arm white(p26), black(p25, false, 0.0005, 180); |
xiaxia686 | 43:c592bf6a6a2d | 267 | while(true) { |
twighk | 1:8119211eae14 | 268 | white(0); |
twighk | 1:8119211eae14 | 269 | black(0); |
twighk | 1:8119211eae14 | 270 | wait(1); |
twighk | 1:8119211eae14 | 271 | white(1); |
twighk | 1:8119211eae14 | 272 | black(1); |
twighk | 1:8119211eae14 | 273 | wait(1); |
twighk | 1:8119211eae14 | 274 | } |
twighk | 1:8119211eae14 | 275 | } |
twighk | 1:8119211eae14 | 276 | |
twighk | 1:8119211eae14 | 277 | |
xiaxia686 | 43:c592bf6a6a2d | 278 | void motorsandservostest() |
xiaxia686 | 43:c592bf6a6a2d | 279 | { |
twighk | 0:200635fa1b08 | 280 | Encoder Eleft(p27, p28), Eright(p30, p29); |
twighk | 0:200635fa1b08 | 281 | MainMotor mleft(p24,p23), mright(p21,p22); |
twighk | 1:8119211eae14 | 282 | Arm sTop(p25), sBottom(p26); |
twighk | 4:1be0f6c6ceae | 283 | //Serial pc(USBTX, USBRX); |
twighk | 0:200635fa1b08 | 284 | const float speed = 0.0; |
twighk | 0:200635fa1b08 | 285 | const float dspeed = 0.0; |
xiaxia686 | 43:c592bf6a6a2d | 286 | |
twighk | 0:200635fa1b08 | 287 | Timer servoTimer; |
xiaxia686 | 43:c592bf6a6a2d | 288 | mleft(speed); |
xiaxia686 | 43:c592bf6a6a2d | 289 | mright(speed); |
twighk | 0:200635fa1b08 | 290 | servoTimer.start(); |
xiaxia686 | 43:c592bf6a6a2d | 291 | while (true) { |
madcowswe | 20:70d651156779 | 292 | printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks()); |
xiaxia686 | 43:c592bf6a6a2d | 293 | if (Eleft.getTicks() < Eright.getTicks()) { |
twighk | 0:200635fa1b08 | 294 | mleft(speed); |
twighk | 0:200635fa1b08 | 295 | mright(speed - dspeed); |
twighk | 0:200635fa1b08 | 296 | } else { |
twighk | 0:200635fa1b08 | 297 | mright(speed); |
twighk | 0:200635fa1b08 | 298 | mleft(speed - dspeed); |
twighk | 0:200635fa1b08 | 299 | } |
xiaxia686 | 43:c592bf6a6a2d | 300 | if (servoTimer.read() < 1) { |
twighk | 0:200635fa1b08 | 301 | sTop.clockwise(); |
twighk | 0:200635fa1b08 | 302 | } else if (servoTimer.read() < 4) { |
twighk | 2:45da48fab346 | 303 | sTop.halt(); |
twighk | 0:200635fa1b08 | 304 | } else if (servoTimer.read() < 5) { |
twighk | 0:200635fa1b08 | 305 | sBottom.anticlockwise(); |
twighk | 0:200635fa1b08 | 306 | //Led=1; |
twighk | 0:200635fa1b08 | 307 | } else if (servoTimer.read() < 6) { |
twighk | 0:200635fa1b08 | 308 | sBottom.clockwise(); |
twighk | 0:200635fa1b08 | 309 | //Led=0; |
twighk | 0:200635fa1b08 | 310 | } else if (servoTimer.read() < 7) { |
twighk | 2:45da48fab346 | 311 | sBottom.halt(); |
xiaxia686 | 43:c592bf6a6a2d | 312 | } else { |
twighk | 0:200635fa1b08 | 313 | sTop.anticlockwise(); |
twighk | 0:200635fa1b08 | 314 | } |
twighk | 0:200635fa1b08 | 315 | if (servoTimer.read() >= 9) servoTimer.reset(); |
twighk | 0:200635fa1b08 | 316 | } |
twighk | 0:200635fa1b08 | 317 | } |
twighk | 0:200635fa1b08 | 318 | |
xiaxia686 | 43:c592bf6a6a2d | 319 | void motortestline() |
xiaxia686 | 43:c592bf6a6a2d | 320 | { |
twighk | 2:45da48fab346 | 321 | MainMotor mleft(p24,p23), mright(p21,p22); |
twighk | 2:45da48fab346 | 322 | const float speed = 0.2; |
xiaxia686 | 43:c592bf6a6a2d | 323 | mleft(speed); |
xiaxia686 | 43:c592bf6a6a2d | 324 | mright(speed); |
twighk | 2:45da48fab346 | 325 | while(true) wait(1); |
twighk | 2:45da48fab346 | 326 | } |
twighk | 2:45da48fab346 | 327 | |
xiaxia686 | 43:c592bf6a6a2d | 328 | void motorencodetestline() |
xiaxia686 | 43:c592bf6a6a2d | 329 | { |
madcowswe | 12:76c9915db820 | 330 | Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B); |
madcowswe | 12:76c9915db820 | 331 | MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); |
twighk | 4:1be0f6c6ceae | 332 | //Serial pc(USBTX, USBRX); |
twighk | 0:200635fa1b08 | 333 | const float speed = 0.2; |
twighk | 0:200635fa1b08 | 334 | const float dspeed = 0.1; |
twighk | 0:200635fa1b08 | 335 | |
xiaxia686 | 43:c592bf6a6a2d | 336 | mleft(speed); |
xiaxia686 | 43:c592bf6a6a2d | 337 | mright(speed); |
xiaxia686 | 43:c592bf6a6a2d | 338 | while (true) { |
xiaxia686 | 43:c592bf6a6a2d | 339 | //left 27 cm = 113 -> 0.239 cm/pulse |
xiaxia686 | 43:c592bf6a6a2d | 340 | //right 27 cm = 72 -> 0.375 cm/pulse |
madcowswe | 20:70d651156779 | 341 | printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375)); |
xiaxia686 | 43:c592bf6a6a2d | 342 | if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375) { |
twighk | 0:200635fa1b08 | 343 | mright(speed - dspeed); |
twighk | 0:200635fa1b08 | 344 | } else { |
twighk | 0:200635fa1b08 | 345 | mright(speed + dspeed); |
twighk | 0:200635fa1b08 | 346 | } |
twighk | 0:200635fa1b08 | 347 | } |
twighk | 0:200635fa1b08 | 348 | |
twighk | 0:200635fa1b08 | 349 | } |
twighk | 0:200635fa1b08 | 350 | |
xiaxia686 | 43:c592bf6a6a2d | 351 | void motorencodetest() |
xiaxia686 | 43:c592bf6a6a2d | 352 | { |
madcowswe | 7:4340355261f9 | 353 | Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B); |
madcowswe | 7:4340355261f9 | 354 | MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); |
twighk | 0:200635fa1b08 | 355 | Serial pc(USBTX, USBRX); |
xiaxia686 | 43:c592bf6a6a2d | 356 | |
twighk | 0:200635fa1b08 | 357 | const float speed = -0.3; |
twighk | 0:200635fa1b08 | 358 | const int enc = -38; |
xiaxia686 | 43:c592bf6a6a2d | 359 | while(true) { |
xiaxia686 | 43:c592bf6a6a2d | 360 | mleft(speed); |
xiaxia686 | 43:c592bf6a6a2d | 361 | mright(0); |
xiaxia686 | 43:c592bf6a6a2d | 362 | while(Eleft.getTicks()>enc) { |
madcowswe | 20:70d651156779 | 363 | printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks()); |
twighk | 0:200635fa1b08 | 364 | } |
xiaxia686 | 43:c592bf6a6a2d | 365 | Eleft.reset(); |
xiaxia686 | 43:c592bf6a6a2d | 366 | Eright.reset(); |
xiaxia686 | 43:c592bf6a6a2d | 367 | mleft(0); |
xiaxia686 | 43:c592bf6a6a2d | 368 | mright(speed); |
xiaxia686 | 43:c592bf6a6a2d | 369 | while(Eright.getTicks()>enc) { |
madcowswe | 20:70d651156779 | 370 | printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks()); |
twighk | 0:200635fa1b08 | 371 | } |
xiaxia686 | 43:c592bf6a6a2d | 372 | Eleft.reset(); |
xiaxia686 | 43:c592bf6a6a2d | 373 | Eright.reset(); |
twighk | 0:200635fa1b08 | 374 | } |
twighk | 0:200635fa1b08 | 375 | } |
twighk | 0:200635fa1b08 | 376 | |
xiaxia686 | 43:c592bf6a6a2d | 377 | void encodertest() |
xiaxia686 | 43:c592bf6a6a2d | 378 | { |
madcowswe | 15:9c5aaeda36dc | 379 | Encoder E1(P_ENC_LEFT_A, P_ENC_LEFT_B); |
madcowswe | 15:9c5aaeda36dc | 380 | //Encoder E2(P_ENC_RIGHT_A, P_ENC_RIGHT_B); |
twighk | 0:200635fa1b08 | 381 | Serial pc(USBTX, USBRX); |
xiaxia686 | 43:c592bf6a6a2d | 382 | while(true) { |
twighk | 0:200635fa1b08 | 383 | wait(0.1); |
madcowswe | 20:70d651156779 | 384 | printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks()); |
twighk | 0:200635fa1b08 | 385 | } |
twighk | 0:200635fa1b08 | 386 | |
twighk | 0:200635fa1b08 | 387 | } |
xiaxia686 | 43:c592bf6a6a2d | 388 | void motortest() |
xiaxia686 | 43:c592bf6a6a2d | 389 | { |
twighk | 0:200635fa1b08 | 390 | MainMotor mright(p22,p21), mleft(p23,p24); |
twighk | 3:717de74f6ebd | 391 | while(true) { |
twighk | 0:200635fa1b08 | 392 | wait(1); |
xiaxia686 | 43:c592bf6a6a2d | 393 | mleft(0.8); |
xiaxia686 | 43:c592bf6a6a2d | 394 | mright(0.8); |
twighk | 0:200635fa1b08 | 395 | wait(1); |
xiaxia686 | 43:c592bf6a6a2d | 396 | mleft(-0.2); |
xiaxia686 | 43:c592bf6a6a2d | 397 | mright(0.2); |
twighk | 0:200635fa1b08 | 398 | wait(1); |
xiaxia686 | 43:c592bf6a6a2d | 399 | mleft(0); |
xiaxia686 | 43:c592bf6a6a2d | 400 | mright(0); |
twighk | 0:200635fa1b08 | 401 | } |
twighk | 0:200635fa1b08 | 402 | } |