Revision 50:937e860f4621, committed 2013-04-12

Comitter:

madcowswe

Date:

Fri Apr 12 21:26:02 2013 +0000

Parent:

49:665bdca0f2cd

Parent:

46:adcd57a5e402

Child:

51:bc261eae004b

Commit message:

Merged oskar and cooper

Changed in this revision

--- a/Processes/AI/ai.cpp	Fri Apr 12 21:07:00 2013 +0000
+++ b/Processes/AI/ai.cpp	Fri Apr 12 21:26:02 2013 +0000
@@ -6,54 +6,25 @@
 void ailayer(void const *dummy)
 {
     Waypoint *current_waypoint = new Waypoint[5];
-/*
-    current_waypoint[0].x = 1;
-    current_waypoint[0].y = 1;
-    current_waypoint[0].theta = 0.0;
-    current_waypoint[0].pos_threshold = 0.05;
-    current_waypoint[0].angle_threshold = 0.05*PI;
-
-    current_waypoint[1].x = 2.2;
-    current_waypoint[1].y = 1.5;
-    current_waypoint[1].theta = PI/2;
-    current_waypoint[1].pos_threshold = 0.05;
-    current_waypoint[1].angle_threshold = 0.05*PI;
-
-    current_waypoint[2].x = -999;
-*/
 
     current_waypoint[0].x = 0.5;
     current_waypoint[0].y = 0.5;
     current_waypoint[0].theta = 0.0;
     current_waypoint[0].pos_threshold = 0.05;
     current_waypoint[0].angle_threshold = 0.05*PI;
-
+    
     current_waypoint[1].x = 2.5;
     current_waypoint[1].y = 0.5;
     current_waypoint[1].theta = 0;
     current_waypoint[1].pos_threshold = 0.05;
     current_waypoint[1].angle_threshold = 0.05*PI;
 
-    current_waypoint[2].x = -999;
-/*
-    //TODO: temp current waypoint hack
-    current_waypoint = new Waypoint;
-    current_waypoint->x = 0.5;
-    current_waypoint->y = 0.7;
-    current_waypoint->theta = 0.0;
-    current_waypoint->pos_threshold = 0.05;
-    current_waypoint->angle_threshold = 0.05*PI;
-    
-    Waypoint* secondwp = new Waypoint;
-    secondwp->x = 1.20;
-    secondwp->y = 0.18;
-    secondwp->theta = PI;
-    secondwp->pos_threshold = 0.01;
-    secondwp->angle_threshold = 0.00001;
-*/    
     motion::setNewWaypoint(current_waypoint);
     
     int currwptidx = 0;
+    Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER, UPPER);
+    Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER, LOWER);
+    
     while(1)
     {
         Thread::wait(50);
@@ -61,9 +32,11 @@
         motion::waypoint_flag_mutex.lock();
         if (motion::checkWaypointStatus())
         {
-            
-            motion::setNewWaypoint(&current_waypoint[++currwptidx % 2]);
-            motion::clearWaypointReached();
+            if(c_upper.getColour() == BLUE && c_lower.getColour() == RED){
+                motion::setNewWaypoint(&current_waypoint[++currwptidx % 2]);
+                motion::clearWaypointReached();
+            }
+
         }
         motion::waypoint_flag_mutex.unlock();
     }

--- a/Processes/AI/ai.h	Fri Apr 12 21:07:00 2013 +0000
+++ b/Processes/AI/ai.h	Fri Apr 12 21:26:02 2013 +0000
@@ -4,6 +4,7 @@
 #include "rtos.h"
 #include "globals.h"
 #include "motion.h"
+#include "Colour.h"
 
 namespace AI
 {

--- a/Sensors/CakeSensor/CakeSensor.h	Fri Apr 12 21:07:00 2013 +0000
+++ b/Sensors/CakeSensor/CakeSensor.h	Fri Apr 12 21:26:02 2013 +0000
@@ -16,6 +16,7 @@
         //float d = 5.5/(Distance()-0.13);
         float d = 7.53/(Distance()-0.022);
         d = (d < 6 || d > 30)? -1:d;
+        
         return d;
     }
 };

--- a/Sensors/Colour/Colour.cpp	Fri Apr 12 21:07:00 2013 +0000
+++ b/Sensors/Colour/Colour.cpp	Fri Apr 12 21:26:02 2013 +0000
@@ -2,29 +2,109 @@
 // Eurobot13 Colour.cpp
 
 #include "Colour.h"
+#include "mbed.h"
 
-void Colour::ReadLed (DigitalOut &led, float &avg, float &stdev, const int measureNum){
-    LedsOff();
-    led = 1;
-    double x = 0, x2 = 0;
-    for (int i = measureNum; i != 0; i--) {
-        float v = pt.read();
-        x += v;
-        x2+= v*v;
+
+Colour::Colour(PinName _blue_led,
+               PinName _red_led,
+               PinName _pt,
+               ArmEnum _arm)
+    : blue_led(_blue_led),
+      red_led(_red_led),
+      pt(_pt),
+      arm(_arm)
+{
+
+
+
+
+
+
+    if (arm == UPPER) {
+        red_correction_factor = UPPERARM_CORRECTION;
+    } else if (arm == LOWER) {
+        red_correction_factor = LOWERARM_CORRECTION;
+    } else {
+        red_correction_factor = 0.00f;
     }
-    avg = x / measureNum;
-    stdev = sqrt(x2 / measureNum - avg*avg);
-    LedsOff();
-    //pc.printf("Phototransistor Analog is: %f\t%f\n\r", avg, stdev);
+
+    togglecolour = 0;
+    blue = 0;
+    red = 0;
+    noise = 0;
+    buff_pointer = 0;
+
+
+    for (int i = 0; i < BUFF_SIZE; i++) {
+        blue_buff[i] = 0;
+        red_buff[i] = 0;
+        noise_buff[i] = 0;
+    }
+
+    ticker.attach(this, &Colour::Blink, 0.01);
+
 }
 
-bool Colour::isColour(DigitalOut &led, const float &avg, const float &stdev, const float numstddev){
-    float avg2, stdev2;
-    ReadLed(led, avg2, stdev2);
+void Colour::Blink (void)
+{
+
+
+    if (togglecolour == 0) {
+
+        float noise_temp = pt.read();
+        noise += (noise_temp - noise_buff[buff_pointer])/BUFF_SIZE;
+        noise_buff[buff_pointer] = noise_temp;
+
+        buff_pointer = (buff_pointer + 1) % BUFF_SIZE;
+
+
+        SNR = 20.0f*log10(hypot(blue,red)/noise);
+
+        float blue_base = (blue - noise);
+        float red_base = (red - noise)*red_correction_factor;
+        colour = atan2(red_base,blue_base);
 
-    if (avg + numstddev*stdev < avg2 - numstddev*stdev2) {
-        return true;
+        //toggles leds for the next state
+        blue_led = 1;
+        red_led = 0;
+    } else if (togglecolour == 1) {
+        float blue_temp = pt.read();
+        blue += (blue_temp - blue_buff[buff_pointer])/BUFF_SIZE;
+        blue_buff[buff_pointer] = blue_temp;
+        //toggles leds for the next state
+        blue_led = 0;
+        red_led = 1;
+    } else if (togglecolour == 2) {
+        float red_temp = pt.read();
+        red += (red_temp - red_buff[buff_pointer])/BUFF_SIZE;
+        red_buff[buff_pointer] = red_temp;
+        //toggles leds for the next state
+        blue_led = 0;
+        red_led = 0;
+    }
+
+
+
+
+    togglecolour = (togglecolour + 1) % 3;
+
+
+}
+
+ColourEnum Colour::getColour()
+{
+    if (SNR > SNR_THRESHOLD_DB) {
+        if ((colour >= -30*PI/180) && (colour < 30*PI/180)) {
+            return BLUE;
+        } else         if ((colour >= 30*PI/180) && (colour < 60*PI/180)) {
+            return WHITE;
+        } else         if ((colour >= 60*PI/180) && (colour < 120*PI/180)) {
+            return RED;
+        } else {
+            return BLACK;
+        }
     } else {
-        return false;
+        return BLACK;
     }
-}
\ No newline at end of file
+
+}

--- a/Sensors/Colour/Colour.h	Fri Apr 12 21:07:00 2013 +0000
+++ b/Sensors/Colour/Colour.h	Fri Apr 12 21:26:02 2013 +0000
@@ -1,51 +1,56 @@
 
 // Eurobot13 Colour.h
-
-//red led use 45ohm
-//blue led use 10ohm
+#ifndef COLOUR_H
+#define COLOUR_H
 
 #include "mbed.h"
+#include "globals.h"
+#include "math.h"
 
-enum ColourEnum {BLUE, RED, WHITE, INCONCLUSIVE, BUG};
+#define BUFF_SIZE 10
+#define SNR_THRESHOLD_DB 4
+
+#define UPPERARM_CORRECTION 2.310f
+#define LOWERARM_CORRECTION 1.000f
+
+
+enum ColourEnum {BLUE=0, RED, WHITE, BLACK};
+enum ArmEnum {UPPER=0, LOWER};
 
 class Colour{
-private:
-    DigitalOut blue;   float bavg, bstdev;
-    DigitalOut red;    float ravg, rstdev; 
-    AnalogIn pt;
-    
 public:
-    Colour(PinName bluePin, PinName redPin, PinName phototransistorPin)
-        : blue(bluePin)
-        , red (redPin)
-        , pt (phototransistorPin)
-        {
-        LedsOff();
-    }
 
-    void Calibrate(){
-        ReadLed(blue, bavg, bstdev);
-        ReadLed( red, ravg, rstdev);
-    }
-        
-    ColourEnum getColour(){
-        bool blueb = isColour(blue, bavg, bstdev)
-            , redb = isColour( red, ravg, rstdev);
-                  
-        if      ( blueb &&  redb)
-                    {return WHITE;}
-        else if ( blueb && !redb)
-                    {return BLUE;}
-        else if (!blueb &&  redb)
-                    {return RED;}
-        else if (!blueb && !redb)
-                    {return INCONCLUSIVE;} 
-        return BUG;            
-    }
+    Colour(
+    PinName blue_led, 
+    PinName red_led,
+    PinName pt,
+    ArmEnum arm);
+    
+    ColourEnum getColour();
+
 
 private:
-    void LedsOff(){blue = 0; red = 0;}
-    void ReadLed (DigitalOut &led, float &avg, float &stdev, const int measureNum = 25); // Colour.cpp
-    bool isColour(DigitalOut &led, const float &avg, const float &stdev, const float numstddev = 2); // Colour.cpp
+    Ticker ticker;
+    DigitalOut blue_led;
+    DigitalOut red_led;
+    AnalogIn pt;
+    ArmEnum arm;
+    
+    float red_correction_factor;
+    float colour;
+    float SNR;
+    void Blink();
     
-};
\ No newline at end of file
+    int togglecolour;
+    float blue;
+    float blue_buff[BUFF_SIZE];
+    float red;
+    float red_buff[BUFF_SIZE];
+    float noise;
+    float noise_buff[BUFF_SIZE];
+    
+    int buff_pointer;    
+    
+};
+
+#endif
\ No newline at end of file

--- a/globals.h	Fri Apr 12 21:07:00 2013 +0000
+++ b/globals.h	Fri Apr 12 21:26:02 2013 +0000
@@ -62,7 +62,8 @@
 const PinName P_DISTANCE_SENSOR = p15;
 const PinName P_FWD_DISTANCE_SENSOR = p16;
 
-const PinName P_COLOR_SENSOR_IN = p20;
+const PinName P_COLOR_SENSOR_IN_UPPER = p20;
+const PinName P_COLOR_SENSOR_IN_LOWER = p19;
 
 const PinName P_MOT_LEFT_A     = p21;
 const PinName P_MOT_LEFT_B     = p22;
@@ -74,8 +75,10 @@
 const PinName P_ENC_LEFT_A      = p27;
 const PinName P_ENC_LEFT_B      = p28;
 
-const PinName P_COLOR_SENSOR_RED = p29;
-const PinName P_COLOR_SENSOR_BLUE = p30;
+const PinName P_COLOR_SENSOR_RED_UPPER = p29;
+const PinName P_COLOR_SENSOR_BLUE_UPPER = p30;
+const PinName P_COLOR_SENSOR_RED_LOWER = p11;
+const PinName P_COLOR_SENSOR_BLUE_LOWER = p10;
 
 
 

--- a/main.cpp	Fri Apr 12 21:07:00 2013 +0000
+++ b/main.cpp	Fri Apr 12 21:26:02 2013 +0000
@@ -22,20 +22,32 @@
 void motorsandservostest();
 void armtest();
 void motortestline();
-void ledtest();
-void phototransistortest();
-void ledphototransistortest();
 void colourtest();
+void pt_test();
 void cakesensortest();
 void printingtestthread(void const*);
 void printingtestthread2(void const*);
 void feedbacktest();
 
-int main() {
-    
-/*****************
- *   Test Code   *
- *****************/
+// bytes packing for peer to peer communication
+typedef union {
+    struct _data {
+        unsigned char header[3];
+        unsigned char ID;
+        float value;
+        float aux;
+    }  data;
+    unsigned char type_char[sizeof(_data)];
+} bytepack_t;
+
+Serial pc(USBTX, USBRX);
+
+int main()
+{
+
+    /*****************
+     *   Test Code   *
+     *****************/
     //motortest();
     //encodertest();
     //motorencodetest();
@@ -49,8 +61,8 @@
     //colourtest(); // Red SnR too low
     //cakesensortest();
     //feedbacktest();
-    
-     /*
+
+    /*
     DigitalOut l1(LED1);
     Thread p(Printing::printingloop,        NULL,   osPriorityNormal,   2048);
     l1=1;
@@ -61,13 +73,11 @@
     
     SystemTime.start();
     
-    Serial pc(USBTX, USBRX);
+    
     pc.baud(115200);
-    
+    wait(2);
     InitSerial();
-    //while(1)
-    //    printbuff();
-    wait(1);
+    wait(3);
     Kalman::KalmanInit();
     
     Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
@@ -77,7 +87,6 @@
     
     Ticker motorcontroltestticker;
     motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
-
     // ai layer thread
     Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
     
@@ -88,14 +97,18 @@
     Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
 
     measureCPUidle(); //repurpose thread for idle measurement
+    //colourtest();
+    //pt_test();
+    while(true) {
     Thread::wait(osWaitForever);
-
+    }
 }
 
 #include <cstdlib>
 using namespace std;
 
-void printingtestthread(void const*){
+void printingtestthread(void const*)
+{
     const char ID = 1;
     float buffer[3] = {ID};
     Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
@@ -108,7 +121,8 @@
     }
 }
 
-void printingtestthread2(void const*){
+void printingtestthread2(void const*)
+{
     const char ID = 2;
     float buffer[5] = {ID};
     Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
@@ -126,117 +140,131 @@
 void feedbacktest(){
     //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
-    
+
     Kalman::State state;
-    
+
     float Pgain = -0.01;
     float fwdspeed = -400/3.0f;
     Timer timer;
     timer.start();
-    
-    while(true){
+
+    while(true) {
         float expecdist = fwdspeed * timer.read();
         state = Kalman::getState();
         float errleft = left_encoder.getTicks() - (expecdist);
         float errright = right_encoder.getTicks() - expecdist;
-        
+
         mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
         mright(max(min(errright*Pgain, 0.4f), -0.4f));
     }
 }
 */
 
-void cakesensortest(){
+void cakesensortest()
+{
     wait(1);
     printf("cakesensortest");
-    
-    CakeSensor cs(P_COLOR_SENSOR_IN);
-    while(true){
+
+    CakeSensor cs(P_DISTANCE_SENSOR);
+    while(true) {
         wait(0.1);
         printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
-        }
+    }
 }
 
-void colourtest(){
-    Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN);
-    c.Calibrate();
-    while(true){
+void colourtest()
+{
+    Colour c_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER);
+    Colour c_lower(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER);
+
+    while(true) {
         wait(0.1);
-        ColourEnum ce = c.getColour();
-        switch(ce){
+
+        switch(c_lower.getColour()) {
             case BLUE :
-                printf("BLUE\n\r");
+                printf("BLUE\n");
                 break;
             case RED:
-                printf("RED\n\r");
+                printf("RED\n");
                 break;
             case WHITE:
-                printf("WHITE\n\r");
+                printf("WHITE\n");
                 break;
-            case INCONCLUSIVE:
-                printf("INCONCLUSIVE\n\r");
+            case BLACK:
+                printf("BLACK\n");
                 break;
             default:
-                printf("BUG\n\r");
+                printf("BUG\n");
         }
+
     }
 
 }
 
-
-void ledphototransistortest(){
-    DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
-    AnalogIn pt(P_COLOR_SENSOR_IN); 
-    Serial pc(USBTX, USBRX);
+void pt_test()
+{
+    DigitalOut _blue_led(p10);
+    DigitalOut _red_led(p11);
+    AnalogIn _pt(p18);
+    
+    bytepack_t datapackage;
+    // first 3 bytes of header is used for alignment
+    datapackage.data.header[0] = 0xFF;
+    datapackage.data.header[1] = 0xFF;
+    datapackage.data.header[2] = 0xFF;
+    while(true) {
+        //toggles leds for the next state
+        _blue_led = 1;
+        _red_led = 0;
+        wait(0.01);
+        volatile float blue_temp = _pt.read();
 
-    while(true){
-        blue = 0; red = 0;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (none): %f \n\r", pt.read());
-        }
-    
-        blue = 1; red = 0;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
-        }
-        blue = 0; red = 1;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
+
+        datapackage.data.ID = (unsigned char)0;
+        datapackage.data.value = blue_temp;
+        datapackage.data.aux = 0;
+        for (int i = 0; i < sizeof(datapackage); i++) {
+            //mbed_serial.putc(datapackage.type_char[i]);
+            pc.putc(datapackage.type_char[i]);
         }
-        blue = 1; red = 1;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (both): %f \n\r", pt.read());
+
+        _blue_led = 0;
+        _red_led = 1;
+        wait(0.01);
+        volatile float red_temp = _pt.read();
+
+
+        datapackage.data.ID = (unsigned char)1;
+        datapackage.data.value = red_temp;
+        datapackage.data.aux = 0;
+        for (int i = 0; i < sizeof(datapackage); i++) {
+            //mbed_serial.putc(datapackage.type_char[i]);
+            pc.putc(datapackage.type_char[i]);
         }
-    } 
-}
 
-void phototransistortest(){
-    AnalogIn pt(P_COLOR_SENSOR_IN); 
-    while(true){
-        wait(0.1);
-        printf("Phototransistor Analog is: %f \n\r", pt.read());
-    }
+        _blue_led = 0;
+        _red_led = 0;
+        wait(0.01);
+        volatile float noise_temp = _pt.read();
 
-}
 
-void ledtest(){
-    DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
-    while(true){
-        blue = 1; red = 0;
-        wait(0.2);
-        blue = 0; red = 1;
-        wait(0.2);
-    
+        datapackage.data.ID = (unsigned char)2;
+        datapackage.data.value = noise_temp;
+        datapackage.data.aux = 0;
+        for (int i = 0; i < sizeof(datapackage); i++) {
+            //mbed_serial.putc(datapackage.type_char[i]);
+            pc.putc(datapackage.type_char[i]);
+        }
+
     }
 }
 
-void armtest(){
+
+
+void armtest()
+{
     Arm white(p26), black(p25, false, 0.0005, 180);
-    while(true){
+    while(true) {
         white(0);
         black(0);
         wait(1);
@@ -247,27 +275,29 @@
 }
 
 
-void motorsandservostest(){
+void motorsandservostest()
+{
     Encoder Eleft(p27, p28), Eright(p30, p29);
     MainMotor mleft(p24,p23), mright(p21,p22);
     Arm sTop(p25), sBottom(p26);
     //Serial pc(USBTX, USBRX);
     const float speed = 0.0;
     const float dspeed = 0.0;
-    
+
     Timer servoTimer;
-    mleft(speed); mright(speed);
+    mleft(speed);
+    mright(speed);
     servoTimer.start();
-    while (true){
+    while (true) {
         printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
-        if (Eleft.getTicks() < Eright.getTicks()){
+        if (Eleft.getTicks() < Eright.getTicks()) {
             mleft(speed);
             mright(speed - dspeed);
         } else {
             mright(speed);
             mleft(speed - dspeed);
         }
-        if (servoTimer.read() < 1){
+        if (servoTimer.read() < 1) {
             sTop.clockwise();
         } else if (servoTimer.read() < 4) {
             sTop.halt();
@@ -279,33 +309,37 @@
             //Led=0;
         } else if (servoTimer.read() < 7) {
             sBottom.halt();
-        }else {
+        } else {
             sTop.anticlockwise();
         }
         if (servoTimer.read() >= 9) servoTimer.reset();
     }
 }
 
-void motortestline(){
+void motortestline()
+{
     MainMotor mleft(p24,p23), mright(p21,p22);
     const float speed = 0.2;
-    mleft(speed); mright(speed);
+    mleft(speed);
+    mright(speed);
     while(true) wait(1);
 }
 
-void motorencodetestline(){
+void motorencodetestline()
+{
     Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
     //Serial pc(USBTX, USBRX);
     const float speed = 0.2;
     const float dspeed = 0.1;
 
-    mleft(speed); mright(speed);
-    while (true){
-    //left 27 cm = 113 -> 0.239 cm/pulse
-    //right 27 cm = 72 -> 0.375 cm/pulse
+    mleft(speed);
+    mright(speed);
+    while (true) {
+        //left 27 cm = 113 -> 0.239 cm/pulse
+        //right 27 cm = 72 -> 0.375 cm/pulse
         printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375));
-        if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375){
+        if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375) {
             mright(speed - dspeed);
         } else {
             mright(speed + dspeed);
@@ -314,45 +348,55 @@
 
 }
 
-void motorencodetest(){
+void motorencodetest()
+{
     Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
     Serial pc(USBTX, USBRX);
-    
+
     const float speed = -0.3;
     const int enc = -38;
-    while(true){
-        mleft(speed); mright(0);
-        while(Eleft.getTicks()>enc){
+    while(true) {
+        mleft(speed);
+        mright(0);
+        while(Eleft.getTicks()>enc) {
             printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
         }
-        Eleft.reset(); Eright.reset();
-        mleft(0); mright(speed);
-        while(Eright.getTicks()>enc){
+        Eleft.reset();
+        Eright.reset();
+        mleft(0);
+        mright(speed);
+        while(Eright.getTicks()>enc) {
             printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
         }
-        Eleft.reset(); Eright.reset();
+        Eleft.reset();
+        Eright.reset();
     }
 }
 
-void encodertest(){
+void encodertest()
+{
     Encoder E1(P_ENC_LEFT_A, P_ENC_LEFT_B);
     //Encoder E2(P_ENC_RIGHT_A, P_ENC_RIGHT_B);
     Serial pc(USBTX, USBRX);
-    while(true){
+    while(true) {
         wait(0.1);
         printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks());
     }
 
 }
-void motortest(){
+void motortest()
+{
     MainMotor mright(p22,p21), mleft(p23,p24);
     while(true) {
         wait(1);
-        mleft(0.8); mright(0.8);
+        mleft(0.8);
+        mright(0.8);
         wait(1);
-        mleft(-0.2); mright(0.2);
+        mleft(-0.2);
+        mright(0.2);
         wait(1);
-        mleft(0); mright(0);
+        mleft(0);
+        mright(0);
     }
 }
\ No newline at end of file