This is some awesome robot code
Dependencies: mbed-rtos mbed QEI
Fork of ICRSEurobot13 by
Diff: main.cpp
- Revision:
- 0:200635fa1b08
- Child:
- 1:8119211eae14
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Mar 29 11:35:34 2013 +0000 @@ -0,0 +1,127 @@ + +// Eurobot13 main.cpp + +#include "mbed.h" + +#include "Actuators/MainMotor/MainMotor.h" +#include "Sensors/Encoder/Encoder.h" +#include "Actuators/Servo/Servo.h" + +PwmOut Led(LED1); + +void motortest(); +void encodertest(); +void motorencodetest(); +void motorencodetestline(); +void motorsandservostest(); + +int main() { + //motortest(); + //encodertest(); + //motorencodetest(); + motorencodetestline(); + //motorsandservostest(); +} + +void motorsandservostest(){ + Encoder Eleft(p27, p28), Eright(p30, p29); + MainMotor mleft(p24,p23), mright(p21,p22); + Servo 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.relax(); + } else if (servoTimer.read() < 5) { + sBottom.anticlockwise(); + //Led=1; + } else if (servoTimer.read() < 6) { + sBottom.clockwise(); + //Led=0; + } else if (servoTimer.read() < 7) { + sBottom.relax(); + }else { + sTop.anticlockwise(); + } + if (servoTimer.read() >= 9) servoTimer.reset(); + } +} + +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 Eleft(p28, p27), Eright(p29, p30); + MainMotor mleft(p23,p24), mright(p22,p21); + 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(1){ + 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(1) { + wait(1); + mleft(0.8); mright(0.8); + wait(1); + mleft(-0.2); mright(0.2); + wait(1); + mleft(0); mright(0); + } +} \ No newline at end of file