2014 Eurobot fork
Dependencies: mbed-rtos mbed QEI
main.cpp
- Committer:
- madcowswe
- Date:
- 2013-04-06
- Revision:
- 14:c638d4b9ee94
- Parent:
- 13:d4b5851742a3
- Parent:
- 12:76c9915db820
- Child:
- 15:9c5aaeda36dc
File content as of revision 14:c638d4b9ee94:
//#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); } #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.002; float fwdspeed = -200/3.0f; Timer timer; timer.start(); while(true){ float expecdist = fwdspeed * timer.read(); float errleft = Eleft.getPoint() - (expecdist*1.07f); 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(p28, p27); Encoder E2(p29, p30); Serial pc(USBTX, USBRX); while(true){ wait(0.1); pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), 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); } }