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@71:eb1956c2d316, 2013-04-15 (annotated)

Committer:
madcowswe
Date:
Mon Apr 15 10:52:36 2013 +0000
Revision:
71:eb1956c2d316
Parent:
67:be3ea5450cc7
Child:
84:00c799fd10a7
Main start cord contraption sends signal 0x2 to ai thread when its time to go

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