main.cpp

00001 #include "globals.h"
00002 #include "Kalman.h"
00003 #include "mbed.h"
00004 #include "rtos.h"
00005 #include "Actuators/Arms/Arm.h"
00006 #include "Actuators/MainMotors/MainMotor.h"
00007 #include "Sensors/Encoders/Encoder.h"
00008 #include "Sensors/Colour/Colour.h"
00009 #include "Sensors/CakeSensor/CakeSensor.h"
00010 #include "Processes/Printing/Printing.h"
00011 #include "coprocserial.h"
00012 #include <algorithm>
00013 
00014 pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
00015 
00016 void motortest();
00017 void encodertest();
00018 void motorencodetest();
00019 void motorencodetestline();
00020 void motorsandservostest();
00021 void armtest();
00022 void motortestline();
00023 void ledtest();
00024 void phototransistortest();
00025 void ledphototransistortest();
00026 void colourtest();
00027 void cakesensortest();
00028 void printingtestthread(void const*);
00029 void printingtestthread2(void const*);
00030 void feedbacktest();
00031 
00032 int main() {
00033     
00034 /*****************
00035  *   Test Code   *
00036  *****************/
00037     //motortest();
00038     //encodertest();
00039     //motorencodetest();
00040     //motorencodetestline();
00041     //motorsandservostest();
00042     //armtest();
00043     //motortestline();
00044     //ledtest();
00045     //phototransistortest();
00046     //ledphototransistortest();
00047     //colourtest(); // Red SnR too low
00048     //cakesensortest();
00049     //feedbacktest();
00050     
00051      /*
00052     DigitalOut l1(LED1);
00053     Thread p(printingThread,        NULL,   osPriorityNormal,   2048);
00054     l1=1;
00055     Thread a(printingtestthread,    NULL,   osPriorityNormal,   1024);
00056     Thread b(printingtestthread2,   NULL,   osPriorityNormal,   1024);
00057     Thread::wait(osWaitForever);
00058     */
00059     
00060     
00061     InitSerial();
00062     //while(1)
00063     //    printbuff();
00064     wait(1);
00065     Kalman::KalmanInit();
00066     
00067     Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
00068     
00069     Kalman::start_predict_ticker(&predictthread);
00070     //Thread::wait(osWaitForever);
00071     feedbacktest();
00072     
00073 }
00074 
00075 #include <cstdlib>
00076 using namespace std;
00077 
00078 void printingtestthread(void const*){
00079     const char ID = 1;
00080     float buffer[3] = {ID};
00081     registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
00082     while (true){
00083         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
00084             buffer[i] = ID ;
00085         }
00086         updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
00087         Thread::wait(200);
00088     }
00089 }
00090 
00091 void printingtestthread2(void const*){
00092     const char ID = 2;
00093     float buffer[5] = {ID};
00094     registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
00095     while (true){
00096         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
00097             buffer[i] = ID;
00098         }
00099         updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
00100         Thread::wait(500);
00101     }
00102 }
00103 
00104 
00105 void feedbacktest(){
00106     //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
00107     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
00108     
00109     Kalman::State state;
00110     
00111     float Pgain = -0.01;
00112     float fwdspeed = -400/3.0f;
00113     Timer timer;
00114     timer.start();
00115     
00116     while(true){
00117         float expecdist = fwdspeed * timer.read();
00118         state = Kalman::getState();
00119         float errleft = left_encoder.getTicks() - (expecdist);
00120         float errright = right_encoder.getTicks() - expecdist;
00121         
00122         mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
00123         mright(max(min(errright*Pgain, 0.4f), -0.4f));
00124     }
00125 }
00126 
00127 void cakesensortest(){
00128     wait(1);
00129     printf("cakesensortest");
00130     
00131     CakeSensor cs(P_COLOR_SENSOR_IN);
00132     while(true){
00133         wait(0.1);
00134         printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
00135         }
00136 }
00137 
00138 void colourtest(){
00139     Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN);
00140     c.Calibrate();
00141     while(true){
00142         wait(0.1);
00143         ColourEnum ce = c.getColour();
00144         switch(ce){
00145             case BLUE :
00146                 printf("BLUE\n\r");
00147                 break;
00148             case RED:
00149                 printf("RED\n\r");
00150                 break;
00151             case WHITE:
00152                 printf("WHITE\n\r");
00153                 break;
00154             case INCONCLUSIVE:
00155                 printf("INCONCLUSIVE\n\r");
00156                 break;
00157             default:
00158                 printf("BUG\n\r");
00159         }
00160     }
00161 
00162 }
00163 
00164 
00165 void ledphototransistortest(){
00166     DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
00167     AnalogIn pt(P_COLOR_SENSOR_IN); 
00168     Serial pc(USBTX, USBRX);
00169 
00170     while(true){
00171         blue = 0; red = 0;
00172         for(int i = 0; i != 5; i++){
00173             wait(0.1);
00174             printf("Phototransistor Analog is (none): %f \n\r", pt.read());
00175         }
00176     
00177         blue = 1; red = 0;
00178         for(int i = 0; i != 5; i++){
00179             wait(0.1);
00180             printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
00181         }
00182         blue = 0; red = 1;
00183         for(int i = 0; i != 5; i++){
00184             wait(0.1);
00185             printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
00186         }
00187         blue = 1; red = 1;
00188         for(int i = 0; i != 5; i++){
00189             wait(0.1);
00190             printf("Phototransistor Analog is (both): %f \n\r", pt.read());
00191         }
00192     } 
00193 }
00194 
00195 void phototransistortest(){
00196     AnalogIn pt(P_COLOR_SENSOR_IN); 
00197     while(true){
00198         wait(0.1);
00199         printf("Phototransistor Analog is: %f \n\r", pt.read());
00200     }
00201 
00202 }
00203 
00204 void ledtest(){
00205     DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
00206     while(true){
00207         blue = 1; red = 0;
00208         wait(0.2);
00209         blue = 0; red = 1;
00210         wait(0.2);
00211     
00212     }
00213 }
00214 
00215 void armtest(){
00216     Arm white(p26), black(p25, false, 0.0005, 180);
00217     while(true){
00218         white(0);
00219         black(0);
00220         wait(1);
00221         white(1);
00222         black(1);
00223         wait(1);
00224     }
00225 }
00226 
00227 
00228 void motorsandservostest(){
00229     Encoder Eleft(p27, p28), Eright(p30, p29);
00230     MainMotor mleft(p24,p23), mright(p21,p22);
00231     Arm sTop(p25), sBottom(p26);
00232     //Serial pc(USBTX, USBRX);
00233     const float speed = 0.0;
00234     const float dspeed = 0.0;
00235     
00236     Timer servoTimer;
00237     mleft(speed); mright(speed);
00238     servoTimer.start();
00239     while (true){
00240         printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
00241         if (Eleft.getTicks() < Eright.getTicks()){
00242             mleft(speed);
00243             mright(speed - dspeed);
00244         } else {
00245             mright(speed);
00246             mleft(speed - dspeed);
00247         }
00248         if (servoTimer.read() < 1){
00249             sTop.clockwise();
00250         } else if (servoTimer.read() < 4) {
00251             sTop.halt();
00252         } else if (servoTimer.read() < 5) {
00253             sBottom.anticlockwise();
00254             //Led=1;
00255         } else if (servoTimer.read() < 6) {
00256             sBottom.clockwise();
00257             //Led=0;
00258         } else if (servoTimer.read() < 7) {
00259             sBottom.halt();
00260         }else {
00261             sTop.anticlockwise();
00262         }
00263         if (servoTimer.read() >= 9) servoTimer.reset();
00264     }
00265 }
00266 
00267 void motortestline(){
00268     MainMotor mleft(p24,p23), mright(p21,p22);
00269     const float speed = 0.2;
00270     mleft(speed); mright(speed);
00271     while(true) wait(1);
00272 }
00273 
00274 void motorencodetestline(){
00275     Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
00276     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
00277     //Serial pc(USBTX, USBRX);
00278     const float speed = 0.2;
00279     const float dspeed = 0.1;
00280 
00281     mleft(speed); mright(speed);
00282     while (true){
00283     //left 27 cm = 113 -> 0.239 cm/pulse
00284     //right 27 cm = 72 -> 0.375 cm/pulse
00285         printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375));
00286         if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375){
00287             mright(speed - dspeed);
00288         } else {
00289             mright(speed + dspeed);
00290         }
00291     }
00292 
00293 }
00294 
00295 void motorencodetest(){
00296     Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
00297     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
00298     Serial pc(USBTX, USBRX);
00299     
00300     const float speed = -0.3;
00301     const int enc = -38;
00302     while(true){
00303         mleft(speed); mright(0);
00304         while(Eleft.getTicks()>enc){
00305             printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
00306         }
00307         Eleft.reset(); Eright.reset();
00308         mleft(0); mright(speed);
00309         while(Eright.getTicks()>enc){
00310             printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
00311         }
00312         Eleft.reset(); Eright.reset();
00313     }
00314 }
00315 
00316 void encodertest(){
00317     Encoder E1(P_ENC_LEFT_A, P_ENC_LEFT_B);
00318     //Encoder E2(P_ENC_RIGHT_A, P_ENC_RIGHT_B);
00319     Serial pc(USBTX, USBRX);
00320     while(true){
00321         wait(0.1);
00322         printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks());
00323     }
00324 
00325 }
00326 void motortest(){
00327     MainMotor mright(p22,p21), mleft(p23,p24);
00328     while(true) {
00329         wait(1);
00330         mleft(0.8); mright(0.8);
00331         wait(1);
00332         mleft(-0.2); mright(0.2);
00333         wait(1);
00334         mleft(0); mright(0);
00335     }
00336 }