Oskar Weigl / Mbed 2 deprecated ICRSEurobot13

This is some awesome robot code

Dependencies:   mbed-rtos mbed QEI

Fork of ICRSEurobot13 by Thomas Branch

main.cpp@90:e4164bb8c60e, 2013-04-17 (annotated)

Committer:
madcowswe
Date:
Wed Apr 17 23:16:25 2013 +0000
Revision:
90:e4164bb8c60e
Parent:
84:00c799fd10a7 
final state at end of competition. Includes avoid wooden team hack

Who changed what in which revision?

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