main.cpp

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