2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
main.cpp@92:4a1225fbb146, 2013-10-15 (annotated)
- Committer:
- rsavitski
- Date:
- Tue Oct 15 12:19:32 2013 +0000
- Revision:
- 92:4a1225fbb146
- Parent:
- 91:fdadfd883825
touch: ripped out 2013-specific bits. Need to address "2014" comments. Rewrite AI layer and other deleted parts.
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 "MainMotor.h" |
madcowswe | 21:167dacfe0b14 | 6 | #include "Encoder.h" |
madcowswe | 21:167dacfe0b14 | 7 | #include "Printing.h" |
madcowswe | 20:70d651156779 | 8 | #include "coprocserial.h" |
madcowswe | 12:76c9915db820 | 9 | #include <algorithm> |
rsavitski | 24:50805ef8c499 | 10 | #include "motion.h" |
madcowswe | 25:b16f1045108f | 11 | #include "MotorControl.h" |
madcowswe | 28:4e20b44251c6 | 12 | #include "system.h" |
rsavitski | 30:791739422122 | 13 | #include "ai.h" |
madcowswe | 20:70d651156779 | 14 | |
twighk | 0:200635fa1b08 | 15 | void motortest(); |
twighk | 0:200635fa1b08 | 16 | void encodertest(); |
twighk | 0:200635fa1b08 | 17 | void motorencodetest(); |
twighk | 0:200635fa1b08 | 18 | void motorencodetestline(); |
twighk | 0:200635fa1b08 | 19 | void motorsandservostest(); |
twighk | 1:8119211eae14 | 20 | void armtest(); |
twighk | 2:45da48fab346 | 21 | void motortestline(); |
twighk | 3:717de74f6ebd | 22 | void colourtest(); |
twighk | 8:69bdf20cb525 | 23 | void cakesensortest(); |
twighk | 13:d4b5851742a3 | 24 | void printingtestthread(void const*); |
twighk | 13:d4b5851742a3 | 25 | void printingtestthread2(void const*); |
madcowswe | 12:76c9915db820 | 26 | void feedbacktest(); |
twighk | 0:200635fa1b08 | 27 | |
madcowswe | 84:00c799fd10a7 | 28 | DigitalOut *balloon_ptr; |
madcowswe | 84:00c799fd10a7 | 29 | |
madcowswe | 84:00c799fd10a7 | 30 | void timeisup_isr(){ |
madcowswe | 84:00c799fd10a7 | 31 | MotorControl::motorsenabled = 0; |
madcowswe | 84:00c799fd10a7 | 32 | *balloon_ptr = 0; |
madcowswe | 84:00c799fd10a7 | 33 | } |
madcowswe | 84:00c799fd10a7 | 34 | |
xiaxia686 | 43:c592bf6a6a2d | 35 | int main() |
xiaxia686 | 43:c592bf6a6a2d | 36 | { |
xiaxia686 | 43:c592bf6a6a2d | 37 | |
xiaxia686 | 43:c592bf6a6a2d | 38 | /***************** |
xiaxia686 | 43:c592bf6a6a2d | 39 | * Test Code * |
xiaxia686 | 43:c592bf6a6a2d | 40 | *****************/ |
twighk | 0:200635fa1b08 | 41 | //motortest(); |
twighk | 0:200635fa1b08 | 42 | //encodertest(); |
twighk | 8:69bdf20cb525 | 43 | //motorencodetest(); |
twighk | 1:8119211eae14 | 44 | //motorencodetestline(); |
twighk | 0:200635fa1b08 | 45 | //motorsandservostest(); |
rsavitski | 65:4709ff6c753c | 46 | //armtest(); |
rsavitski | 65:4709ff6c753c | 47 | //while(1); |
twighk | 2:45da48fab346 | 48 | //motortestline(); |
twighk | 11:bbddc908c78c | 49 | //ledtest(); |
twighk | 3:717de74f6ebd | 50 | //phototransistortest(); |
madcowswe | 12:76c9915db820 | 51 | //ledphototransistortest(); |
madcowswe | 5:56a5fdd373c9 | 52 | //colourtest(); // Red SnR too low |
madcowswe | 12:76c9915db820 | 53 | //cakesensortest(); |
madcowswe | 20:70d651156779 | 54 | //feedbacktest(); |
xiaxia686 | 43:c592bf6a6a2d | 55 | |
xiaxia686 | 43:c592bf6a6a2d | 56 | /* |
twighk | 13:d4b5851742a3 | 57 | DigitalOut l1(LED1); |
madcowswe | 21:167dacfe0b14 | 58 | Thread p(Printing::printingloop, NULL, osPriorityNormal, 2048); |
twighk | 13:d4b5851742a3 | 59 | l1=1; |
twighk | 13:d4b5851742a3 | 60 | Thread a(printingtestthread, NULL, osPriorityNormal, 1024); |
twighk | 13:d4b5851742a3 | 61 | Thread b(printingtestthread2, NULL, osPriorityNormal, 1024); |
twighk | 13:d4b5851742a3 | 62 | Thread::wait(osWaitForever); |
madcowswe | 15:9c5aaeda36dc | 63 | */ |
madcowswe | 16:52250d8d8fce | 64 | |
madcowswe | 22:6e3218cf75f8 | 65 | SystemTime.start(); |
madcowswe | 20:70d651156779 | 66 | |
madcowswe | 51:bc261eae004b | 67 | Serial pc(USBTX, USBRX); |
madcowswe | 25:b16f1045108f | 68 | pc.baud(115200); |
xiaxia686 | 46:adcd57a5e402 | 69 | InitSerial(); |
xiaxia686 | 46:adcd57a5e402 | 70 | wait(3); |
xiaxia686 | 46:adcd57a5e402 | 71 | Kalman::KalmanInit(); |
madcowswe | 20:70d651156779 | 72 | |
xiaxia686 | 46:adcd57a5e402 | 73 | Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k |
xiaxia686 | 46:adcd57a5e402 | 74 | Kalman::start_predict_ticker(&predictthread); |
madcowswe | 20:70d651156779 | 75 | |
xiaxia686 | 46:adcd57a5e402 | 76 | Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084); |
rsavitski | 24:50805ef8c499 | 77 | |
xiaxia686 | 46:adcd57a5e402 | 78 | Ticker motorcontroltestticker; |
xiaxia686 | 46:adcd57a5e402 | 79 | motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05); |
rsavitski | 30:791739422122 | 80 | // ai layer thread |
rsavitski | 91:fdadfd883825 | 81 | //Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048); //2014: add new ai layer |
madcowswe | 22:6e3218cf75f8 | 82 | |
rsavitski | 24:50805ef8c499 | 83 | // motion layer periodic callback |
xiaxia686 | 46:adcd57a5e402 | 84 | RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic); |
xiaxia686 | 46:adcd57a5e402 | 85 | motion_timer.start(50); |
xiaxia686 | 43:c592bf6a6a2d | 86 | |
xiaxia686 | 46:adcd57a5e402 | 87 | Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048); |
madcowswe | 71:eb1956c2d316 | 88 | |
madcowswe | 84:00c799fd10a7 | 89 | //worry about starting |
madcowswe | 71:eb1956c2d316 | 90 | DigitalIn startcord(P_START_CORD); |
madcowswe | 71:eb1956c2d316 | 91 | startcord.mode(PullUp); |
madcowswe | 71:eb1956c2d316 | 92 | while(!startcord) |
madcowswe | 71:eb1956c2d316 | 93 | Thread::wait(50); |
madcowswe | 84:00c799fd10a7 | 94 | |
madcowswe | 84:00c799fd10a7 | 95 | //worry about stopping |
madcowswe | 84:00c799fd10a7 | 96 | DigitalOut balloon(P_BALLOON); |
madcowswe | 84:00c799fd10a7 | 97 | balloon = 1; |
madcowswe | 84:00c799fd10a7 | 98 | balloon_ptr = &balloon; |
madcowswe | 84:00c799fd10a7 | 99 | Timeout timeout_90s; |
madcowswe | 84:00c799fd10a7 | 100 | timeout_90s.attach(timeisup_isr, 90); |
madcowswe | 84:00c799fd10a7 | 101 | |
rsavitski | 91:fdadfd883825 | 102 | //aithread.signal_set(0x2); //Tell AI thread that its time to go //2014 |
madcowswe | 71:eb1956c2d316 | 103 | |
madcowswe | 84:00c799fd10a7 | 104 | |
xiaxia686 | 46:adcd57a5e402 | 105 | measureCPUidle(); //repurpose thread for idle measurement |
madcowswe | 64:c979fb1cd3b5 | 106 | /* |
madcowswe | 62:78d99b781f02 | 107 | MotorControl::set_omegacmd(0); |
madcowswe | 62:78d99b781f02 | 108 | for(float spd = 0.02; spd <= 0.5; spd *= 1.4){ |
madcowswe | 62:78d99b781f02 | 109 | |
madcowswe | 62:78d99b781f02 | 110 | MotorControl::set_fwdcmd(spd); |
madcowswe | 62:78d99b781f02 | 111 | |
madcowswe | 62:78d99b781f02 | 112 | Thread::wait(3000); |
madcowswe | 62:78d99b781f02 | 113 | |
madcowswe | 62:78d99b781f02 | 114 | float f = MotorControl::mfwdpowdbg; |
madcowswe | 62:78d99b781f02 | 115 | float r = MotorControl::mrotpowdbg; |
madcowswe | 62:78d99b781f02 | 116 | MotorControl::set_fwdcmd(0); |
madcowswe | 62:78d99b781f02 | 117 | printf("targetspd:%f, fwd:%f, rot:%f\r\n", spd, f, r); |
madcowswe | 62:78d99b781f02 | 118 | Thread::wait(5000); |
madcowswe | 62:78d99b781f02 | 119 | } |
madcowswe | 62:78d99b781f02 | 120 | |
madcowswe | 63:c2c6269767b8 | 121 | MotorControl::set_fwdcmd(0); |
madcowswe | 63:c2c6269767b8 | 122 | for(float spd = 0.05; spd <= 2; spd *= 1.4){ |
madcowswe | 63:c2c6269767b8 | 123 | |
madcowswe | 63:c2c6269767b8 | 124 | MotorControl::set_omegacmd(spd); |
madcowswe | 63:c2c6269767b8 | 125 | |
madcowswe | 63:c2c6269767b8 | 126 | Thread::wait(3000); |
madcowswe | 63:c2c6269767b8 | 127 | |
madcowswe | 63:c2c6269767b8 | 128 | float f = MotorControl::mfwdpowdbg; |
madcowswe | 63:c2c6269767b8 | 129 | float r = MotorControl::mrotpowdbg; |
madcowswe | 63:c2c6269767b8 | 130 | MotorControl::set_omegacmd(0); |
madcowswe | 63:c2c6269767b8 | 131 | printf("targetspd:%f, fwd:%f, rot:%f\r\n", spd, f, r); |
madcowswe | 63:c2c6269767b8 | 132 | Thread::wait(5000); |
madcowswe | 63:c2c6269767b8 | 133 | } |
madcowswe | 64:c979fb1cd3b5 | 134 | */ |
madcowswe | 21:167dacfe0b14 | 135 | Thread::wait(osWaitForever); |
madcowswe | 51:bc261eae004b | 136 | |
madcowswe | 12:76c9915db820 | 137 | } |
madcowswe | 12:76c9915db820 | 138 | |
twighk | 13:d4b5851742a3 | 139 | #include <cstdlib> |
twighk | 13:d4b5851742a3 | 140 | using namespace std; |
twighk | 13:d4b5851742a3 | 141 | |
xiaxia686 | 43:c592bf6a6a2d | 142 | void printingtestthread(void const*) |
xiaxia686 | 43:c592bf6a6a2d | 143 | { |
twighk | 13:d4b5851742a3 | 144 | const char ID = 1; |
twighk | 13:d4b5851742a3 | 145 | float buffer[3] = {ID}; |
madcowswe | 21:167dacfe0b14 | 146 | Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0])); |
twighk | 13:d4b5851742a3 | 147 | while (true){ |
twighk | 13:d4b5851742a3 | 148 | for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){ |
madcowswe | 20:70d651156779 | 149 | buffer[i] = ID ; |
twighk | 13:d4b5851742a3 | 150 | } |
madcowswe | 21:167dacfe0b14 | 151 | Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0])); |
twighk | 13:d4b5851742a3 | 152 | Thread::wait(200); |
twighk | 13:d4b5851742a3 | 153 | } |
twighk | 13:d4b5851742a3 | 154 | } |
madcowswe | 14:c638d4b9ee94 | 155 | |
xiaxia686 | 43:c592bf6a6a2d | 156 | void printingtestthread2(void const*) |
xiaxia686 | 43:c592bf6a6a2d | 157 | { |
twighk | 13:d4b5851742a3 | 158 | const char ID = 2; |
twighk | 13:d4b5851742a3 | 159 | float buffer[5] = {ID}; |
madcowswe | 21:167dacfe0b14 | 160 | Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0])); |
twighk | 13:d4b5851742a3 | 161 | while (true){ |
twighk | 13:d4b5851742a3 | 162 | for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){ |
twighk | 13:d4b5851742a3 | 163 | buffer[i] = ID; |
twighk | 13:d4b5851742a3 | 164 | } |
madcowswe | 21:167dacfe0b14 | 165 | Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0])); |
twighk | 13:d4b5851742a3 | 166 | Thread::wait(500); |
twighk | 13:d4b5851742a3 | 167 | } |
twighk | 8:69bdf20cb525 | 168 | } |
twighk | 8:69bdf20cb525 | 169 | |
madcowswe | 14:c638d4b9ee94 | 170 | |
rsavitski | 24:50805ef8c499 | 171 | /* |
madcowswe | 12:76c9915db820 | 172 | void feedbacktest(){ |
madcowswe | 20:70d651156779 | 173 | //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B); |
madcowswe | 12:76c9915db820 | 174 | MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); |
xiaxia686 | 43:c592bf6a6a2d | 175 | |
madcowswe | 20:70d651156779 | 176 | Kalman::State state; |
xiaxia686 | 43:c592bf6a6a2d | 177 | |
madcowswe | 20:70d651156779 | 178 | float Pgain = -0.01; |
madcowswe | 15:9c5aaeda36dc | 179 | float fwdspeed = -400/3.0f; |
madcowswe | 12:76c9915db820 | 180 | Timer timer; |
madcowswe | 12:76c9915db820 | 181 | timer.start(); |
xiaxia686 | 43:c592bf6a6a2d | 182 | |
xiaxia686 | 43:c592bf6a6a2d | 183 | while(true) { |
madcowswe | 12:76c9915db820 | 184 | float expecdist = fwdspeed * timer.read(); |
madcowswe | 20:70d651156779 | 185 | state = Kalman::getState(); |
madcowswe | 20:70d651156779 | 186 | float errleft = left_encoder.getTicks() - (expecdist); |
madcowswe | 20:70d651156779 | 187 | float errright = right_encoder.getTicks() - expecdist; |
xiaxia686 | 43:c592bf6a6a2d | 188 | |
madcowswe | 12:76c9915db820 | 189 | mleft(max(min(errleft*Pgain, 0.4f), -0.4f)); |
madcowswe | 12:76c9915db820 | 190 | mright(max(min(errright*Pgain, 0.4f), -0.4f)); |
madcowswe | 12:76c9915db820 | 191 | } |
twighk | 8:69bdf20cb525 | 192 | } |
rsavitski | 24:50805ef8c499 | 193 | */ |
rsavitski | 91:fdadfd883825 | 194 | /* |
xiaxia686 | 43:c592bf6a6a2d | 195 | void cakesensortest() |
xiaxia686 | 43:c592bf6a6a2d | 196 | { |
twighk | 8:69bdf20cb525 | 197 | wait(1); |
madcowswe | 20:70d651156779 | 198 | printf("cakesensortest"); |
xiaxia686 | 43:c592bf6a6a2d | 199 | |
xiaxia686 | 43:c592bf6a6a2d | 200 | CakeSensor cs(P_DISTANCE_SENSOR); |
xiaxia686 | 43:c592bf6a6a2d | 201 | while(true) { |
twighk | 8:69bdf20cb525 | 202 | wait(0.1); |
madcowswe | 20:70d651156779 | 203 | printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm()); |
xiaxia686 | 43:c592bf6a6a2d | 204 | } |
twighk | 3:717de74f6ebd | 205 | } |
twighk | 3:717de74f6ebd | 206 | |
xiaxia686 | 43:c592bf6a6a2d | 207 | void colourtest() |
xiaxia686 | 43:c592bf6a6a2d | 208 | { |
xiaxia686 | 45:77cf6375348a | 209 | Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER); |
xiaxia686 | 45:77cf6375348a | 210 | Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER); |
xiaxia686 | 43:c592bf6a6a2d | 211 | |
xiaxia686 | 43:c592bf6a6a2d | 212 | while(true) { |
twighk | 3:717de74f6ebd | 213 | wait(0.1); |
xiaxia686 | 43:c592bf6a6a2d | 214 | |
xiaxia686 | 45:77cf6375348a | 215 | switch(c_lower.getColour()) { |
twighk | 3:717de74f6ebd | 216 | case BLUE : |
xiaxia686 | 43:c592bf6a6a2d | 217 | printf("BLUE\n"); |
twighk | 3:717de74f6ebd | 218 | break; |
twighk | 3:717de74f6ebd | 219 | case RED: |
xiaxia686 | 43:c592bf6a6a2d | 220 | printf("RED\n"); |
twighk | 3:717de74f6ebd | 221 | break; |
twighk | 3:717de74f6ebd | 222 | case WHITE: |
xiaxia686 | 43:c592bf6a6a2d | 223 | printf("WHITE\n"); |
twighk | 3:717de74f6ebd | 224 | break; |
xiaxia686 | 43:c592bf6a6a2d | 225 | case BLACK: |
xiaxia686 | 43:c592bf6a6a2d | 226 | printf("BLACK\n"); |
twighk | 3:717de74f6ebd | 227 | break; |
twighk | 3:717de74f6ebd | 228 | default: |
xiaxia686 | 43:c592bf6a6a2d | 229 | printf("BUG\n"); |
twighk | 3:717de74f6ebd | 230 | } |
xiaxia686 | 43:c592bf6a6a2d | 231 | |
twighk | 2:45da48fab346 | 232 | } |
twighk | 0:200635fa1b08 | 233 | |
twighk | 3:717de74f6ebd | 234 | } |
rsavitski | 91:fdadfd883825 | 235 | */ |
madcowswe | 51:bc261eae004b | 236 | /* |
madcowswe | 51:bc261eae004b | 237 | |
xiaxia686 | 43:c592bf6a6a2d | 238 | void pt_test() |
xiaxia686 | 43:c592bf6a6a2d | 239 | { |
xiaxia686 | 43:c592bf6a6a2d | 240 | DigitalOut _blue_led(p10); |
xiaxia686 | 43:c592bf6a6a2d | 241 | DigitalOut _red_led(p11); |
xiaxia686 | 45:77cf6375348a | 242 | AnalogIn _pt(p18); |
xiaxia686 | 43:c592bf6a6a2d | 243 | |
xiaxia686 | 43:c592bf6a6a2d | 244 | bytepack_t datapackage; |
xiaxia686 | 43:c592bf6a6a2d | 245 | // first 3 bytes of header is used for alignment |
xiaxia686 | 43:c592bf6a6a2d | 246 | datapackage.data.header[0] = 0xFF; |
xiaxia686 | 43:c592bf6a6a2d | 247 | datapackage.data.header[1] = 0xFF; |
xiaxia686 | 43:c592bf6a6a2d | 248 | datapackage.data.header[2] = 0xFF; |
xiaxia686 | 43:c592bf6a6a2d | 249 | while(true) { |
xiaxia686 | 43:c592bf6a6a2d | 250 | //toggles leds for the next state |
xiaxia686 | 43:c592bf6a6a2d | 251 | _blue_led = 1; |
xiaxia686 | 43:c592bf6a6a2d | 252 | _red_led = 0; |
xiaxia686 | 43:c592bf6a6a2d | 253 | wait(0.01); |
xiaxia686 | 43:c592bf6a6a2d | 254 | volatile float blue_temp = _pt.read(); |
twighk | 3:717de74f6ebd | 255 | |
xiaxia686 | 43:c592bf6a6a2d | 256 | |
xiaxia686 | 43:c592bf6a6a2d | 257 | datapackage.data.ID = (unsigned char)0; |
xiaxia686 | 43:c592bf6a6a2d | 258 | datapackage.data.value = blue_temp; |
xiaxia686 | 43:c592bf6a6a2d | 259 | datapackage.data.aux = 0; |
xiaxia686 | 43:c592bf6a6a2d | 260 | for (int i = 0; i < sizeof(datapackage); i++) { |
xiaxia686 | 43:c592bf6a6a2d | 261 | //mbed_serial.putc(datapackage.type_char[i]); |
xiaxia686 | 43:c592bf6a6a2d | 262 | pc.putc(datapackage.type_char[i]); |
twighk | 3:717de74f6ebd | 263 | } |
xiaxia686 | 43:c592bf6a6a2d | 264 | |
xiaxia686 | 43:c592bf6a6a2d | 265 | _blue_led = 0; |
xiaxia686 | 43:c592bf6a6a2d | 266 | _red_led = 1; |
xiaxia686 | 43:c592bf6a6a2d | 267 | wait(0.01); |
xiaxia686 | 43:c592bf6a6a2d | 268 | volatile float red_temp = _pt.read(); |
xiaxia686 | 43:c592bf6a6a2d | 269 | |
xiaxia686 | 43:c592bf6a6a2d | 270 | |
xiaxia686 | 43:c592bf6a6a2d | 271 | datapackage.data.ID = (unsigned char)1; |
xiaxia686 | 43:c592bf6a6a2d | 272 | datapackage.data.value = red_temp; |
xiaxia686 | 43:c592bf6a6a2d | 273 | datapackage.data.aux = 0; |
xiaxia686 | 43:c592bf6a6a2d | 274 | for (int i = 0; i < sizeof(datapackage); i++) { |
xiaxia686 | 43:c592bf6a6a2d | 275 | //mbed_serial.putc(datapackage.type_char[i]); |
xiaxia686 | 43:c592bf6a6a2d | 276 | pc.putc(datapackage.type_char[i]); |
twighk | 11:bbddc908c78c | 277 | } |
twighk | 3:717de74f6ebd | 278 | |
xiaxia686 | 43:c592bf6a6a2d | 279 | _blue_led = 0; |
xiaxia686 | 43:c592bf6a6a2d | 280 | _red_led = 0; |
xiaxia686 | 43:c592bf6a6a2d | 281 | wait(0.01); |
xiaxia686 | 43:c592bf6a6a2d | 282 | volatile float noise_temp = _pt.read(); |
twighk | 3:717de74f6ebd | 283 | |
twighk | 3:717de74f6ebd | 284 | |
xiaxia686 | 43:c592bf6a6a2d | 285 | datapackage.data.ID = (unsigned char)2; |
xiaxia686 | 43:c592bf6a6a2d | 286 | datapackage.data.value = noise_temp; |
xiaxia686 | 43:c592bf6a6a2d | 287 | datapackage.data.aux = 0; |
xiaxia686 | 43:c592bf6a6a2d | 288 | for (int i = 0; i < sizeof(datapackage); i++) { |
xiaxia686 | 43:c592bf6a6a2d | 289 | //mbed_serial.putc(datapackage.type_char[i]); |
xiaxia686 | 43:c592bf6a6a2d | 290 | pc.putc(datapackage.type_char[i]); |
xiaxia686 | 43:c592bf6a6a2d | 291 | } |
xiaxia686 | 43:c592bf6a6a2d | 292 | |
twighk | 3:717de74f6ebd | 293 | } |
twighk | 3:717de74f6ebd | 294 | } |
madcowswe | 51:bc261eae004b | 295 | */ |
rsavitski | 65:4709ff6c753c | 296 | #ifdef AGDHGADSYIGYJDGA |
rsavitski | 65:4709ff6c753c | 297 | PwmOut white(p26); |
rsavitski | 65:4709ff6c753c | 298 | PwmOut black(p25); |
xiaxia686 | 43:c592bf6a6a2d | 299 | |
rsavitski | 65:4709ff6c753c | 300 | void armtest() |
rsavitski | 65:4709ff6c753c | 301 | { |
rsavitski | 65:4709ff6c753c | 302 | |
rsavitski | 65:4709ff6c753c | 303 | white.period(0.05); |
rsavitski | 65:4709ff6c753c | 304 | black.period(0.05); |
rsavitski | 65:4709ff6c753c | 305 | |
rsavitski | 65:4709ff6c753c | 306 | /* float f=1; |
rsavitski | 65:4709ff6c753c | 307 | for (f=1; f<3; f+=0.1) |
rsavitski | 65:4709ff6c753c | 308 | { |
rsavitski | 65:4709ff6c753c | 309 | black.pulsewidth_us(f*1000); |
rsavitski | 65:4709ff6c753c | 310 | wait(1); |
rsavitski | 65:4709ff6c753c | 311 | printf("%f\r\n", f); |
rsavitski | 65:4709ff6c753c | 312 | } |
rsavitski | 65:4709ff6c753c | 313 | for (f=2; f>0; f-=0.1) |
rsavitski | 65:4709ff6c753c | 314 | { |
rsavitski | 65:4709ff6c753c | 315 | black.pulsewidth_us(f*1000); |
rsavitski | 65:4709ff6c753c | 316 | wait(1); |
rsavitski | 65:4709ff6c753c | 317 | printf("%f\r\n", f); |
rsavitski | 65:4709ff6c753c | 318 | }*/ |
rsavitski | 65:4709ff6c753c | 319 | |
rsavitski | 65:4709ff6c753c | 320 | |
rsavitski | 65:4709ff6c753c | 321 | for(;;) |
rsavitski | 65:4709ff6c753c | 322 | { |
rsavitski | 65:4709ff6c753c | 323 | black.pulsewidth_us(2.0*1000); |
rsavitski | 65:4709ff6c753c | 324 | wait(2); |
rsavitski | 65:4709ff6c753c | 325 | black.pulsewidth_us(0.9*1000);//1.2 |
rsavitski | 65:4709ff6c753c | 326 | wait(2); |
rsavitski | 65:4709ff6c753c | 327 | } |
rsavitski | 65:4709ff6c753c | 328 | |
rsavitski | 65:4709ff6c753c | 329 | // white works |
rsavitski | 65:4709ff6c753c | 330 | /*for(;;) |
rsavitski | 65:4709ff6c753c | 331 | { |
rsavitski | 65:4709ff6c753c | 332 | white.pulsewidth_us(0.6*1000); |
rsavitski | 65:4709ff6c753c | 333 | wait(2); |
rsavitski | 65:4709ff6c753c | 334 | white.pulsewidth_us(2.5*1000); |
rsavitski | 65:4709ff6c753c | 335 | wait(2); |
rsavitski | 65:4709ff6c753c | 336 | }*/ |
rsavitski | 65:4709ff6c753c | 337 | |
rsavitski | 65:4709ff6c753c | 338 | /* while(1) //2.5 -> //0.6 |
rsavitski | 65:4709ff6c753c | 339 | { |
rsavitski | 65:4709ff6c753c | 340 | white.pulsewidth_us(int(f*1000)); |
rsavitski | 65:4709ff6c753c | 341 | printf("%f\r\n", f); |
rsavitski | 65:4709ff6c753c | 342 | f-=0.1; |
rsavitski | 65:4709ff6c753c | 343 | wait(1); |
rsavitski | 65:4709ff6c753c | 344 | }*/ |
rsavitski | 65:4709ff6c753c | 345 | } |
rsavitski | 65:4709ff6c753c | 346 | #endif |
rsavitski | 65:4709ff6c753c | 347 | #ifdef FSDHGFSJDF |
xiaxia686 | 43:c592bf6a6a2d | 348 | void armtest() |
xiaxia686 | 43:c592bf6a6a2d | 349 | { |
rsavitski | 65:4709ff6c753c | 350 | Arm lower_arm(p25, 0.05, 0.9, 2.0); |
rsavitski | 65:4709ff6c753c | 351 | Arm upper_arm(p26, 0.05, 0.6, 2.5); |
rsavitski | 65:4709ff6c753c | 352 | |
rsavitski | 65:4709ff6c753c | 353 | while(1) |
rsavitski | 65:4709ff6c753c | 354 | { |
rsavitski | 65:4709ff6c753c | 355 | upper_arm.go_up(); |
rsavitski | 65:4709ff6c753c | 356 | wait(2); |
rsavitski | 65:4709ff6c753c | 357 | upper_arm.go_down(); |
rsavitski | 65:4709ff6c753c | 358 | wait(2); |
twighk | 1:8119211eae14 | 359 | } |
twighk | 1:8119211eae14 | 360 | } |
rsavitski | 65:4709ff6c753c | 361 | #endif |
rsavitski | 65:4709ff6c753c | 362 | void armtest(){} |
rsavitski | 65:4709ff6c753c | 363 | /* |
xiaxia686 | 43:c592bf6a6a2d | 364 | void motorsandservostest() |
xiaxia686 | 43:c592bf6a6a2d | 365 | { |
twighk | 0:200635fa1b08 | 366 | Encoder Eleft(p27, p28), Eright(p30, p29); |
twighk | 0:200635fa1b08 | 367 | MainMotor mleft(p24,p23), mright(p21,p22); |
twighk | 1:8119211eae14 | 368 | Arm sTop(p25), sBottom(p26); |
twighk | 4:1be0f6c6ceae | 369 | //Serial pc(USBTX, USBRX); |
twighk | 0:200635fa1b08 | 370 | const float speed = 0.0; |
twighk | 0:200635fa1b08 | 371 | const float dspeed = 0.0; |
xiaxia686 | 43:c592bf6a6a2d | 372 | |
twighk | 0:200635fa1b08 | 373 | Timer servoTimer; |
xiaxia686 | 43:c592bf6a6a2d | 374 | mleft(speed); |
xiaxia686 | 43:c592bf6a6a2d | 375 | mright(speed); |
twighk | 0:200635fa1b08 | 376 | servoTimer.start(); |
xiaxia686 | 43:c592bf6a6a2d | 377 | while (true) { |
madcowswe | 20:70d651156779 | 378 | printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks()); |
xiaxia686 | 43:c592bf6a6a2d | 379 | if (Eleft.getTicks() < Eright.getTicks()) { |
twighk | 0:200635fa1b08 | 380 | mleft(speed); |
twighk | 0:200635fa1b08 | 381 | mright(speed - dspeed); |
twighk | 0:200635fa1b08 | 382 | } else { |
twighk | 0:200635fa1b08 | 383 | mright(speed); |
twighk | 0:200635fa1b08 | 384 | mleft(speed - dspeed); |
twighk | 0:200635fa1b08 | 385 | } |
xiaxia686 | 43:c592bf6a6a2d | 386 | if (servoTimer.read() < 1) { |
twighk | 0:200635fa1b08 | 387 | sTop.clockwise(); |
twighk | 0:200635fa1b08 | 388 | } else if (servoTimer.read() < 4) { |
twighk | 2:45da48fab346 | 389 | sTop.halt(); |
twighk | 0:200635fa1b08 | 390 | } else if (servoTimer.read() < 5) { |
twighk | 0:200635fa1b08 | 391 | sBottom.anticlockwise(); |
twighk | 0:200635fa1b08 | 392 | //Led=1; |
twighk | 0:200635fa1b08 | 393 | } else if (servoTimer.read() < 6) { |
twighk | 0:200635fa1b08 | 394 | sBottom.clockwise(); |
twighk | 0:200635fa1b08 | 395 | //Led=0; |
twighk | 0:200635fa1b08 | 396 | } else if (servoTimer.read() < 7) { |
twighk | 2:45da48fab346 | 397 | sBottom.halt(); |
xiaxia686 | 43:c592bf6a6a2d | 398 | } else { |
twighk | 0:200635fa1b08 | 399 | sTop.anticlockwise(); |
twighk | 0:200635fa1b08 | 400 | } |
twighk | 0:200635fa1b08 | 401 | if (servoTimer.read() >= 9) servoTimer.reset(); |
twighk | 0:200635fa1b08 | 402 | } |
twighk | 0:200635fa1b08 | 403 | } |
rsavitski | 65:4709ff6c753c | 404 | */ |
xiaxia686 | 43:c592bf6a6a2d | 405 | void motortestline() |
xiaxia686 | 43:c592bf6a6a2d | 406 | { |
twighk | 2:45da48fab346 | 407 | MainMotor mleft(p24,p23), mright(p21,p22); |
twighk | 2:45da48fab346 | 408 | const float speed = 0.2; |
xiaxia686 | 43:c592bf6a6a2d | 409 | mleft(speed); |
xiaxia686 | 43:c592bf6a6a2d | 410 | mright(speed); |
twighk | 2:45da48fab346 | 411 | while(true) wait(1); |
twighk | 2:45da48fab346 | 412 | } |
twighk | 2:45da48fab346 | 413 | |
xiaxia686 | 43:c592bf6a6a2d | 414 | void motorencodetestline() |
xiaxia686 | 43:c592bf6a6a2d | 415 | { |
madcowswe | 12:76c9915db820 | 416 | Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B); |
madcowswe | 12:76c9915db820 | 417 | MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); |
twighk | 4:1be0f6c6ceae | 418 | //Serial pc(USBTX, USBRX); |
twighk | 0:200635fa1b08 | 419 | const float speed = 0.2; |
twighk | 0:200635fa1b08 | 420 | const float dspeed = 0.1; |
twighk | 0:200635fa1b08 | 421 | |
xiaxia686 | 43:c592bf6a6a2d | 422 | mleft(speed); |
xiaxia686 | 43:c592bf6a6a2d | 423 | mright(speed); |
xiaxia686 | 43:c592bf6a6a2d | 424 | while (true) { |
xiaxia686 | 43:c592bf6a6a2d | 425 | //left 27 cm = 113 -> 0.239 cm/pulse |
xiaxia686 | 43:c592bf6a6a2d | 426 | //right 27 cm = 72 -> 0.375 cm/pulse |
madcowswe | 20:70d651156779 | 427 | printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375)); |
xiaxia686 | 43:c592bf6a6a2d | 428 | if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375) { |
twighk | 0:200635fa1b08 | 429 | mright(speed - dspeed); |
twighk | 0:200635fa1b08 | 430 | } else { |
twighk | 0:200635fa1b08 | 431 | mright(speed + dspeed); |
twighk | 0:200635fa1b08 | 432 | } |
twighk | 0:200635fa1b08 | 433 | } |
twighk | 0:200635fa1b08 | 434 | |
twighk | 0:200635fa1b08 | 435 | } |
twighk | 0:200635fa1b08 | 436 | |
xiaxia686 | 43:c592bf6a6a2d | 437 | void motorencodetest() |
xiaxia686 | 43:c592bf6a6a2d | 438 | { |
madcowswe | 7:4340355261f9 | 439 | Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B); |
madcowswe | 7:4340355261f9 | 440 | MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B); |
twighk | 0:200635fa1b08 | 441 | Serial pc(USBTX, USBRX); |
xiaxia686 | 43:c592bf6a6a2d | 442 | |
twighk | 0:200635fa1b08 | 443 | const float speed = -0.3; |
twighk | 0:200635fa1b08 | 444 | const int enc = -38; |
xiaxia686 | 43:c592bf6a6a2d | 445 | while(true) { |
xiaxia686 | 43:c592bf6a6a2d | 446 | mleft(speed); |
xiaxia686 | 43:c592bf6a6a2d | 447 | mright(0); |
xiaxia686 | 43:c592bf6a6a2d | 448 | while(Eleft.getTicks()>enc) { |
madcowswe | 20:70d651156779 | 449 | printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks()); |
twighk | 0:200635fa1b08 | 450 | } |
xiaxia686 | 43:c592bf6a6a2d | 451 | Eleft.reset(); |
xiaxia686 | 43:c592bf6a6a2d | 452 | Eright.reset(); |
xiaxia686 | 43:c592bf6a6a2d | 453 | mleft(0); |
xiaxia686 | 43:c592bf6a6a2d | 454 | mright(speed); |
xiaxia686 | 43:c592bf6a6a2d | 455 | while(Eright.getTicks()>enc) { |
madcowswe | 20:70d651156779 | 456 | printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks()); |
twighk | 0:200635fa1b08 | 457 | } |
xiaxia686 | 43:c592bf6a6a2d | 458 | Eleft.reset(); |
xiaxia686 | 43:c592bf6a6a2d | 459 | Eright.reset(); |
twighk | 0:200635fa1b08 | 460 | } |
twighk | 0:200635fa1b08 | 461 | } |
twighk | 0:200635fa1b08 | 462 | |
xiaxia686 | 43:c592bf6a6a2d | 463 | void encodertest() |
xiaxia686 | 43:c592bf6a6a2d | 464 | { |
madcowswe | 15:9c5aaeda36dc | 465 | Encoder E1(P_ENC_LEFT_A, P_ENC_LEFT_B); |
madcowswe | 15:9c5aaeda36dc | 466 | //Encoder E2(P_ENC_RIGHT_A, P_ENC_RIGHT_B); |
twighk | 0:200635fa1b08 | 467 | Serial pc(USBTX, USBRX); |
xiaxia686 | 43:c592bf6a6a2d | 468 | while(true) { |
twighk | 0:200635fa1b08 | 469 | wait(0.1); |
madcowswe | 20:70d651156779 | 470 | printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks()); |
twighk | 0:200635fa1b08 | 471 | } |
twighk | 0:200635fa1b08 | 472 | |
twighk | 0:200635fa1b08 | 473 | } |
xiaxia686 | 43:c592bf6a6a2d | 474 | void motortest() |
xiaxia686 | 43:c592bf6a6a2d | 475 | { |
twighk | 0:200635fa1b08 | 476 | MainMotor mright(p22,p21), mleft(p23,p24); |
twighk | 3:717de74f6ebd | 477 | while(true) { |
twighk | 0:200635fa1b08 | 478 | wait(1); |
xiaxia686 | 43:c592bf6a6a2d | 479 | mleft(0.8); |
xiaxia686 | 43:c592bf6a6a2d | 480 | mright(0.8); |
twighk | 0:200635fa1b08 | 481 | wait(1); |
xiaxia686 | 43:c592bf6a6a2d | 482 | mleft(-0.2); |
xiaxia686 | 43:c592bf6a6a2d | 483 | mright(0.2); |
twighk | 0:200635fa1b08 | 484 | wait(1); |
xiaxia686 | 43:c592bf6a6a2d | 485 | mleft(0); |
xiaxia686 | 43:c592bf6a6a2d | 486 | mright(0); |
twighk | 0:200635fa1b08 | 487 | } |
twighk | 0:200635fa1b08 | 488 | } |