![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
main.cpp
- Committer:
- twighk
- Date:
- 2013-04-05
- Revision:
- 10:1f0cf0182067
- Parent:
- 7:4340355261f9
- Parent:
- 8:69bdf20cb525
- Child:
- 11:bbddc908c78c
File content as of revision 10:1f0cf0182067:
//#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 */ #define P_SERVO_LOWER_ARM p5 #define P_SERVO_UPPER_ARM p6 #define P_SERIAL_RX p14 #define P_DISTANCE_SENSOR p15 #define P_COLOR_SENSOR_IN p20 #define P_MOT_RIGHT_A p21 #define P_MOT_RIGHT_B p22 #define P_MOT_LEFT_A p23 #define P_MOT_LEFT_B p24 #define P_ENC_RIGHT_A p28 #define P_ENC_RIGHT_B p27 #define P_ENC_LEFT_A p25 #define P_ENC_LEFT_B p26 #define P_COLOR_SENSOR_RED p29 #define P_COLOR_SENSOR_BLUE p30 #include "mbed.h" Serial pc(USBTX, USBRX); #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" void motortest(); void encodertest(); void motorencodetest(); void motorencodetestline(); void motorsandservostest(); void armtest(); void motortestline(); void ledtest(); void phototransistortest(); void ledphototransistortest(); void colourtest(); void cakesensortest(); int main() { /***************** * Test Code * *****************/ //motortest(); //encodertest(); //motorencodetest(); //motorencodetestline(); //motorsandservostest(); //armtest(); //motortestline(); ledtest(); //phototransistortest(); //ledphototransistortest(); //colourtest(); // Red SnR too low cakesensortest(); } void cakesensortest(){ wait(1); pc.printf("cakesensortest"); CakeSensor cs(p20); 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 = 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()); } } } 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 Eleft(p27, p28), Eright(p30, p29); MainMotor mleft(p24,p23), mright(p21,p22); //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); } }