File content as of revision 16:52250d8d8fce:

//#pragma Otime // Compiler Optimisations

// Eurobot13 main.cpp



/*
PINOUT Sensors
5:  RF:SDI
6 SDO
7 SCK
8 NCS
9 NIRQ
10-15 6 echo pins
16 trig
17 IRin
18-20 unused
21 stepper step
22-27 unused
28 Serial TX
29-30 unused


PINOUT Main
5: Lower arm servo
6: Upper arm servo

14: Serial RX
15: Distance sensor

20: color sensor in
21-24: Motors PWM IN 1-4
25-26: Encoders 
27-28: Encoders 
29: Color sensor RED LED
30: Color sensor BLUE LED

*/
#include "mbed.h"
#include "rtos.h"
Serial pc(USBTX, USBRX);

const PinName P_SERVO_LOWER_ARM = p5;
const PinName P_SERVO_UPPER_ARM = p6;

const PinName P_SERIAL_RX       = p14;
const PinName P_DISTANCE_SENSOR = p15;

const PinName P_COLOR_SENSOR_IN = p20;

const PinName P_MOT_RIGHT_A     = p21;
const PinName P_MOT_RIGHT_B     = p22;
const PinName P_MOT_LEFT_A      = p23;
const PinName P_MOT_LEFT_B      = p24;

const PinName P_ENC_RIGHT_A     = p28;
const PinName P_ENC_RIGHT_B     = p27;
const PinName P_ENC_LEFT_A      = p25;
const PinName P_ENC_LEFT_B      = p26;

const PinName P_COLOR_SENSOR_RED = p29;
const PinName P_COLOR_SENSOR_BLUE = p30;

#include "Actuators/Arms/Arm.h"
#include "Actuators/MainMotors/MainMotor.h"
#include "Sensors/Encoders/Encoder.h"
#include "Sensors/Colour/Colour.h"
#include "Sensors/CakeSensor/CakeSensor.h"
#include "Processes/Printing/Printing.h"
#include <algorithm>

void motortest();
void encodertest();
void motorencodetest();
void motorencodetestline();
void motorsandservostest();
void armtest();
void motortestline();
void ledtest();
void phototransistortest();
void ledphototransistortest();
void colourtest();
void cakesensortest();
void printingtestthread(void const*);
void printingtestthread2(void const*);
void feedbacktest();

int main() {
    
/*****************
 *   Test Code   *
 *****************/
    //motortest();
    //encodertest();
    //motorencodetest();
    //motorencodetestline();
    //motorsandservostest();
    //armtest();
    //motortestline();
    //ledtest();
    //phototransistortest();
    //ledphototransistortest();
    //colourtest(); // Red SnR too low
    //cakesensortest();
    feedbacktest();
    
    /*
    DigitalOut l1(LED1);
    Thread p(printingThread,        NULL,   osPriorityNormal,   2048);
    l1=1;
    Thread a(printingtestthread,    NULL,   osPriorityNormal,   1024);
    Thread b(printingtestthread2,   NULL,   osPriorityNormal,   1024);
    Thread::wait(osWaitForever);
    */
    
    //Kalman test threads
    //Ticker predictticker;
    //predictthread(predictloopwrapper, this, osPriorityNormal, 512)
    //updatethread(updateloopwrapper, this, osPriorityNormal, 512)
    //predictticker( SIGTICKARGS(predictthread, 0x1) ),
}

#include <cstdlib>
using namespace std;

void printingtestthread(void const*){
    const char ID = 1;
    float buffer[3] = {ID};
    registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
    while (true){
        for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
            buffer[i] =ID ;
        }
        updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
        Thread::wait(200);
    }
}

void printingtestthread2(void const*){
    const char ID = 2;
    float buffer[5] = {ID};
    registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
    while (true){
        for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
            buffer[i] = ID;
        }
        updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
        Thread::wait(500);
    }
}


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);
    
    float Pgain = -0.005;
    float fwdspeed = -400/3.0f;
    Timer timer;
    timer.start();
    
    while(true){
        float expecdist = fwdspeed * timer.read();
        float errleft = Eleft.getPoint() - (expecdist*1.05);
        float errright = Eright.getPoint() - expecdist;
        
        mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
        mright(max(min(errright*Pgain, 0.4f), -0.4f));
    }
}

void cakesensortest(){
    wait(1);
    pc.printf("cakesensortest");
    
    CakeSensor cs(P_COLOR_SENSOR_IN);
    while(true){
        wait(0.1);
        pc.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){
        wait(0.1);
        ColourEnum ce = c.getColour();
        switch(ce){
            case BLUE :
                pc.printf("BLUE\n\r");
                break;
            case RED:
                pc.printf("RED\n\r");
                break;
            case WHITE:
                pc.printf("WHITE\n\r");
                break;
            case INCONCLUSIVE:
                pc.printf("INCONCLUSIVE\n\r");
                break;
            default:
                pc.printf("BUG\n\r");
        }
    }

}


void ledphototransistortest(){
    DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
    AnalogIn pt(P_COLOR_SENSOR_IN); 
    Serial pc(USBTX, USBRX);

    while(true){
        blue = 0; red = 0;
        for(int i = 0; i != 5; i++){
            wait(0.1);
            pc.printf("Phototransistor Analog is (none): %f \n\r", pt.read());
        }
    
        blue = 1; red = 0;
        for(int i = 0; i != 5; i++){
            wait(0.1);
            pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
        }
        blue = 0; red = 1;
        for(int i = 0; i != 5; i++){
            wait(0.1);
            pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
        }
        blue = 1; red = 1;
        for(int i = 0; i != 5; i++){
            wait(0.1);
            pc.printf("Phototransistor Analog is (both): %f \n\r", pt.read());
        }
    } 
}

void phototransistortest(){
    AnalogIn pt(P_COLOR_SENSOR_IN); 
    while(true){
        wait(0.1);
        pc.printf("Phototransistor Analog is: %f \n\r", 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);
    
    }
}

void armtest(){
    Arm white(p26), black(p25, false, 0.0005, 180);
    while(true){
        white(0);
        black(0);
        wait(1);
        white(1);
        black(1);
        wait(1);
    }
}


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);
    servoTimer.start();
    while (true){
        pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
        if (Eleft.getPoint() < Eright.getPoint()){
            mleft(speed);
            mright(speed - dspeed);
        } else {
            mright(speed);
            mleft(speed - dspeed);
        }
        if (servoTimer.read() < 1){
            sTop.clockwise();
        } else if (servoTimer.read() < 4) {
            sTop.halt();
        } else if (servoTimer.read() < 5) {
            sBottom.anticlockwise();
            //Led=1;
        } else if (servoTimer.read() < 6) {
            sBottom.clockwise();
            //Led=0;
        } else if (servoTimer.read() < 7) {
            sBottom.halt();
        }else {
            sTop.anticlockwise();
        }
        if (servoTimer.read() >= 9) servoTimer.reset();
    }
}

void motortestline(){
    MainMotor mleft(p24,p23), mright(p21,p22);
    const float speed = 0.2;
    mleft(speed); mright(speed);
    while(true) wait(1);
}

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
        pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
        if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
            mright(speed - dspeed);
        } else {
            mright(speed + dspeed);
        }
    }

}

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.getPoint()>enc){
            pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
        }
        Eleft.reset(); Eright.reset();
        mleft(0); mright(speed);
        while(Eright.getPoint()>enc){
            pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
        }
        Eleft.reset(); Eright.reset();
    }
}

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){
        wait(0.1);
        pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), 0);//E2.getPoint());
    }

}
void motortest(){
    MainMotor mright(p22,p21), mleft(p23,p24);
    while(true) {
        wait(1);
        mleft(0.8); mright(0.8);
        wait(1);
        mleft(-0.2); mright(0.2);
        wait(1);
        mleft(0); mright(0);
    }
}