main.cpp

00001 #include "globals.h"
00002 #include "Kalman.h"
00003 #include "mbed.h"
00004 #include "rtos.h"
00005 #include "Arm.h"
00006 #include "MainMotor.h"
00007 #include "Encoder.h"
00008 #include "Colour.h"
00009 #include "CakeSensor.h"
00010 #include "Printing.h"
00011 #include "coprocserial.h"
00012 #include <algorithm>
00013 #include "motion.h"
00014 #include "MotorControl.h"
00015 #include "system.h"
00016 #include "ai.h"
00017 
00018 void motortest();
00019 void encodertest();
00020 void motorencodetest();
00021 void motorencodetestline();
00022 void motorsandservostest();
00023 void armtest();
00024 void motortestline();
00025 void colourtest();
00026 void cakesensortest();
00027 void printingtestthread(void const*);
00028 void printingtestthread2(void const*);
00029 void feedbacktest();
00030 
00031 DigitalOut *balloon_ptr;
00032 
00033 void timeisup_isr(){
00034     MotorControl::motorsenabled = 0;
00035     *balloon_ptr = 0;
00036 }
00037 
00038 int main()
00039 {
00040 
00041     /*****************
00042      *   Test Code   *
00043      *****************/
00044     //motortest();
00045     //encodertest();
00046     //motorencodetest();
00047     //motorencodetestline();
00048     //motorsandservostest();
00049     //armtest();
00050     //while(1);
00051     //motortestline();
00052     //ledtest();
00053     //phototransistortest();
00054     //ledphototransistortest();
00055     //colourtest(); // Red SnR too low
00056     //cakesensortest();
00057     //feedbacktest();
00058 
00059     /*
00060     DigitalOut l1(LED1);
00061     Thread p(Printing::printingloop,        NULL,   osPriorityNormal,   2048);
00062     l1=1;
00063     Thread a(printingtestthread,    NULL,   osPriorityNormal,   1024);
00064     Thread b(printingtestthread2,   NULL,   osPriorityNormal,   1024);
00065     Thread::wait(osWaitForever);
00066     */
00067     
00068     SystemTime.start();
00069     
00070     Serial pc(USBTX, USBRX);
00071     pc.baud(115200);
00072     InitSerial();
00073     wait(3);
00074     Kalman::KalmanInit();
00075     
00076     Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
00077     Kalman::start_predict_ticker(&predictthread);
00078     
00079     Thread updatethread(Kalman::updateloop, NULL, osPriorityNormal, 2084);
00080     
00081     Ticker motorcontroltestticker;
00082     motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
00083     // ai layer thread
00084     Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
00085     
00086     // motion layer periodic callback
00087     RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
00088     motion_timer.start(50);
00089 
00090     Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
00091     
00092     //worry about starting
00093     DigitalIn startcord(P_START_CORD);
00094     startcord.mode(PullUp);
00095     while(!startcord)
00096         Thread::wait(50);
00097         
00098     //worry about stopping
00099     DigitalOut balloon(P_BALLOON);
00100     balloon = 1;
00101     balloon_ptr = &balloon;
00102     Timeout timeout_90s;
00103     timeout_90s.attach(timeisup_isr, 90);
00104     
00105     aithread.signal_set(0x2); //Tell AI thread that its time to go
00106     
00107     
00108     measureCPUidle(); //repurpose thread for idle measurement
00109     /*
00110     MotorControl::set_omegacmd(0);
00111     for(float spd = 0.02; spd <= 0.5; spd *= 1.4){
00112     
00113         MotorControl::set_fwdcmd(spd);
00114         
00115         Thread::wait(3000);
00116         
00117         float f = MotorControl::mfwdpowdbg;
00118         float r = MotorControl::mrotpowdbg;
00119         MotorControl::set_fwdcmd(0);
00120         printf("targetspd:%f, fwd:%f, rot:%f\r\n", spd, f, r);
00121         Thread::wait(5000);
00122     }
00123     
00124     MotorControl::set_fwdcmd(0);
00125     for(float spd = 0.05; spd <= 2; spd *= 1.4){
00126     
00127         MotorControl::set_omegacmd(spd);
00128         
00129         Thread::wait(3000);
00130         
00131         float f = MotorControl::mfwdpowdbg;
00132         float r = MotorControl::mrotpowdbg;
00133         MotorControl::set_omegacmd(0);
00134         printf("targetspd:%f, fwd:%f, rot:%f\r\n", spd, f, r);
00135         Thread::wait(5000);
00136     }
00137     */
00138     Thread::wait(osWaitForever);
00139    
00140 }
00141 
00142 #include <cstdlib>
00143 using namespace std;
00144 
00145 void printingtestthread(void const*)
00146 {
00147     const char ID = 1;
00148     float buffer[3] = {ID};
00149     Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
00150     while (true){
00151         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
00152             buffer[i] = ID ;
00153         }
00154         Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
00155         Thread::wait(200);
00156     }
00157 }
00158 
00159 void printingtestthread2(void const*)
00160 {
00161     const char ID = 2;
00162     float buffer[5] = {ID};
00163     Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
00164     while (true){
00165         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
00166             buffer[i] = ID;
00167         }
00168         Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
00169         Thread::wait(500);
00170     }
00171 }
00172 
00173 
00174 /*
00175 void feedbacktest(){
00176     //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
00177     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
00178 
00179     Kalman::State state;
00180 
00181     float Pgain = -0.01;
00182     float fwdspeed = -400/3.0f;
00183     Timer timer;
00184     timer.start();
00185 
00186     while(true) {
00187         float expecdist = fwdspeed * timer.read();
00188         state = Kalman::getState();
00189         float errleft = left_encoder.getTicks() - (expecdist);
00190         float errright = right_encoder.getTicks() - expecdist;
00191 
00192         mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
00193         mright(max(min(errright*Pgain, 0.4f), -0.4f));
00194     }
00195 }
00196 */
00197 
00198 void cakesensortest()
00199 {
00200     wait(1);
00201     printf("cakesensortest");
00202 
00203     CakeSensor cs(P_DISTANCE_SENSOR);
00204     while(true) {
00205         wait(0.1);
00206         printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
00207     }
00208 }
00209 
00210 void colourtest()
00211 {
00212     Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER);
00213     Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER);
00214 
00215     while(true) {
00216         wait(0.1);
00217 
00218         switch(c_lower.getColour()) {
00219             case BLUE :
00220                 printf("BLUE\n");
00221                 break;
00222             case RED:
00223                 printf("RED\n");
00224                 break;
00225             case WHITE:
00226                 printf("WHITE\n");
00227                 break;
00228             case BLACK:
00229                 printf("BLACK\n");
00230                 break;
00231             default:
00232                 printf("BUG\n");
00233         }
00234 
00235     }
00236 
00237 }
00238 
00239 /*
00240 
00241 void pt_test()
00242 {
00243     DigitalOut _blue_led(p10);
00244     DigitalOut _red_led(p11);
00245     AnalogIn _pt(p18);
00246     
00247     bytepack_t datapackage;
00248     // first 3 bytes of header is used for alignment
00249     datapackage.data.header[0] = 0xFF;
00250     datapackage.data.header[1] = 0xFF;
00251     datapackage.data.header[2] = 0xFF;
00252     while(true) {
00253         //toggles leds for the next state
00254         _blue_led = 1;
00255         _red_led = 0;
00256         wait(0.01);
00257         volatile float blue_temp = _pt.read();
00258 
00259 
00260         datapackage.data.ID = (unsigned char)0;
00261         datapackage.data.value = blue_temp;
00262         datapackage.data.aux = 0;
00263         for (int i = 0; i < sizeof(datapackage); i++) {
00264             //mbed_serial.putc(datapackage.type_char[i]);
00265             pc.putc(datapackage.type_char[i]);
00266         }
00267 
00268         _blue_led = 0;
00269         _red_led = 1;
00270         wait(0.01);
00271         volatile float red_temp = _pt.read();
00272 
00273 
00274         datapackage.data.ID = (unsigned char)1;
00275         datapackage.data.value = red_temp;
00276         datapackage.data.aux = 0;
00277         for (int i = 0; i < sizeof(datapackage); i++) {
00278             //mbed_serial.putc(datapackage.type_char[i]);
00279             pc.putc(datapackage.type_char[i]);
00280         }
00281 
00282         _blue_led = 0;
00283         _red_led = 0;
00284         wait(0.01);
00285         volatile float noise_temp = _pt.read();
00286 
00287 
00288         datapackage.data.ID = (unsigned char)2;
00289         datapackage.data.value = noise_temp;
00290         datapackage.data.aux = 0;
00291         for (int i = 0; i < sizeof(datapackage); i++) {
00292             //mbed_serial.putc(datapackage.type_char[i]);
00293             pc.putc(datapackage.type_char[i]);
00294         }
00295 
00296     }
00297 }
00298 */
00299 #ifdef AGDHGADSYIGYJDGA
00300 PwmOut white(p26);
00301 PwmOut black(p25);
00302 
00303 void armtest() 
00304 {    
00305     
00306     white.period(0.05);
00307     black.period(0.05);
00308     
00309   /*  float f=1;
00310     for (f=1; f<3; f+=0.1)
00311     { 
00312         black.pulsewidth_us(f*1000);
00313         wait(1);
00314         printf("%f\r\n", f);
00315     }
00316     for (f=2; f>0; f-=0.1)
00317     { 
00318         black.pulsewidth_us(f*1000);
00319         wait(1);
00320         printf("%f\r\n", f);
00321     }*/
00322     
00323     
00324     for(;;)
00325     {
00326         black.pulsewidth_us(2.0*1000);
00327         wait(2);
00328         black.pulsewidth_us(0.9*1000);//1.2
00329         wait(2);
00330     }
00331     
00332     // white works
00333     /*for(;;)
00334     {
00335         white.pulsewidth_us(0.6*1000);
00336         wait(2);
00337         white.pulsewidth_us(2.5*1000);
00338         wait(2);
00339     }*/
00340     
00341    /* while(1) //2.5 -> //0.6
00342     {
00343         white.pulsewidth_us(int(f*1000));
00344         printf("%f\r\n", f);
00345         f-=0.1;
00346         wait(1);
00347     }*/
00348 }
00349 #endif
00350 #ifdef FSDHGFSJDF
00351 void armtest()
00352 {
00353     Arm lower_arm(p25, 0.05, 0.9, 2.0);
00354     Arm upper_arm(p26, 0.05, 0.6, 2.5);
00355     
00356     while(1)
00357     {
00358         upper_arm.go_up();
00359         wait(2);
00360         upper_arm.go_down();
00361         wait(2);
00362     }
00363 }
00364 #endif
00365 void armtest(){}
00366 /*
00367 void motorsandservostest()
00368 {
00369     Encoder Eleft(p27, p28), Eright(p30, p29);
00370     MainMotor mleft(p24,p23), mright(p21,p22);
00371     Arm sTop(p25), sBottom(p26);
00372     //Serial pc(USBTX, USBRX);
00373     const float speed = 0.0;
00374     const float dspeed = 0.0;
00375 
00376     Timer servoTimer;
00377     mleft(speed);
00378     mright(speed);
00379     servoTimer.start();
00380     while (true) {
00381         printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
00382         if (Eleft.getTicks() < Eright.getTicks()) {
00383             mleft(speed);
00384             mright(speed - dspeed);
00385         } else {
00386             mright(speed);
00387             mleft(speed - dspeed);
00388         }
00389         if (servoTimer.read() < 1) {
00390             sTop.clockwise();
00391         } else if (servoTimer.read() < 4) {
00392             sTop.halt();
00393         } else if (servoTimer.read() < 5) {
00394             sBottom.anticlockwise();
00395             //Led=1;
00396         } else if (servoTimer.read() < 6) {
00397             sBottom.clockwise();
00398             //Led=0;
00399         } else if (servoTimer.read() < 7) {
00400             sBottom.halt();
00401         } else {
00402             sTop.anticlockwise();
00403         }
00404         if (servoTimer.read() >= 9) servoTimer.reset();
00405     }
00406 }
00407 */
00408 void motortestline()
00409 {
00410     MainMotor mleft(p24,p23), mright(p21,p22);
00411     const float speed = 0.2;
00412     mleft(speed);
00413     mright(speed);
00414     while(true) wait(1);
00415 }
00416 
00417 void motorencodetestline()
00418 {
00419     Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
00420     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
00421     //Serial pc(USBTX, USBRX);
00422     const float speed = 0.2;
00423     const float dspeed = 0.1;
00424 
00425     mleft(speed);
00426     mright(speed);
00427     while (true) {
00428         //left 27 cm = 113 -> 0.239 cm/pulse
00429         //right 27 cm = 72 -> 0.375 cm/pulse
00430         printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375));
00431         if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375) {
00432             mright(speed - dspeed);
00433         } else {
00434             mright(speed + dspeed);
00435         }
00436     }
00437 
00438 }
00439 
00440 void motorencodetest()
00441 {
00442     Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
00443     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
00444     Serial pc(USBTX, USBRX);
00445 
00446     const float speed = -0.3;
00447     const int enc = -38;
00448     while(true) {
00449         mleft(speed);
00450         mright(0);
00451         while(Eleft.getTicks()>enc) {
00452             printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
00453         }
00454         Eleft.reset();
00455         Eright.reset();
00456         mleft(0);
00457         mright(speed);
00458         while(Eright.getTicks()>enc) {
00459             printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
00460         }
00461         Eleft.reset();
00462         Eright.reset();
00463     }
00464 }
00465 
00466 void encodertest()
00467 {
00468     Encoder E1(P_ENC_LEFT_A, P_ENC_LEFT_B);
00469     //Encoder E2(P_ENC_RIGHT_A, P_ENC_RIGHT_B);
00470     Serial pc(USBTX, USBRX);
00471     while(true) {
00472         wait(0.1);
00473         printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks());
00474     }
00475 
00476 }
00477 void motortest()
00478 {
00479     MainMotor mright(p22,p21), mleft(p23,p24);
00480     while(true) {
00481         wait(1);
00482         mleft(0.8);
00483         mright(0.8);
00484         wait(1);
00485         mleft(-0.2);
00486         mright(0.2);
00487         wait(1);
00488         mleft(0);
00489         mright(0);
00490     }
00491 }