Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

main.cpp@0:200635fa1b08, 2013-03-29 (annotated)

Committer:
twighk
Date:
Fri Mar 29 11:35:34 2013 +0000
Revision:
0:200635fa1b08
Child:
1:8119211eae14
Main motors and motor encoders

Who changed what in which revision?

UserRevisionLine numberNew contents of line
twighk 0:200635fa1b08 1
twighk 0:200635fa1b08 2 // Eurobot13 main.cpp
twighk 0:200635fa1b08 3
twighk 0:200635fa1b08 4 #include "mbed.h"
twighk 0:200635fa1b08 5
twighk 0:200635fa1b08 6 #include "Actuators/MainMotor/MainMotor.h"
twighk 0:200635fa1b08 7 #include "Sensors/Encoder/Encoder.h"
twighk 0:200635fa1b08 8 #include "Actuators/Servo/Servo.h"
twighk 0:200635fa1b08 9
twighk 0:200635fa1b08 10 PwmOut Led(LED1);
twighk 0:200635fa1b08 11
twighk 0:200635fa1b08 12 void motortest();
twighk 0:200635fa1b08 13 void encodertest();
twighk 0:200635fa1b08 14 void motorencodetest();
twighk 0:200635fa1b08 15 void motorencodetestline();
twighk 0:200635fa1b08 16 void motorsandservostest();
twighk 0:200635fa1b08 17
twighk 0:200635fa1b08 18 int main() {
twighk 0:200635fa1b08 19 //motortest();
twighk 0:200635fa1b08 20 //encodertest();
twighk 0:200635fa1b08 21 //motorencodetest();
twighk 0:200635fa1b08 22 motorencodetestline();
twighk 0:200635fa1b08 23 //motorsandservostest();
twighk 0:200635fa1b08 24 }
twighk 0:200635fa1b08 25
twighk 0:200635fa1b08 26 void motorsandservostest(){
twighk 0:200635fa1b08 27 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 28 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 0:200635fa1b08 29 Servo sTop(p25), sBottom(p26);
twighk 0:200635fa1b08 30 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 31 const float speed = 0.0;
twighk 0:200635fa1b08 32 const float dspeed = 0.0;
twighk 0:200635fa1b08 33
twighk 0:200635fa1b08 34 Timer servoTimer;
twighk 0:200635fa1b08 35 mleft(speed); mright(speed);
twighk 0:200635fa1b08 36 servoTimer.start();
twighk 0:200635fa1b08 37 while (true){
twighk 0:200635fa1b08 38 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 39 if (Eleft.getPoint() < Eright.getPoint()){
twighk 0:200635fa1b08 40 mleft(speed);
twighk 0:200635fa1b08 41 mright(speed - dspeed);
twighk 0:200635fa1b08 42 } else {
twighk 0:200635fa1b08 43 mright(speed);
twighk 0:200635fa1b08 44 mleft(speed - dspeed);
twighk 0:200635fa1b08 45 }
twighk 0:200635fa1b08 46 if (servoTimer.read() < 1){
twighk 0:200635fa1b08 47 sTop.clockwise();
twighk 0:200635fa1b08 48 } else if (servoTimer.read() < 4) {
twighk 0:200635fa1b08 49 sTop.relax();
twighk 0:200635fa1b08 50 } else if (servoTimer.read() < 5) {
twighk 0:200635fa1b08 51 sBottom.anticlockwise();
twighk 0:200635fa1b08 52 //Led=1;
twighk 0:200635fa1b08 53 } else if (servoTimer.read() < 6) {
twighk 0:200635fa1b08 54 sBottom.clockwise();
twighk 0:200635fa1b08 55 //Led=0;
twighk 0:200635fa1b08 56 } else if (servoTimer.read() < 7) {
twighk 0:200635fa1b08 57 sBottom.relax();
twighk 0:200635fa1b08 58 }else {
twighk 0:200635fa1b08 59 sTop.anticlockwise();
twighk 0:200635fa1b08 60 }
twighk 0:200635fa1b08 61 if (servoTimer.read() >= 9) servoTimer.reset();
twighk 0:200635fa1b08 62 }
twighk 0:200635fa1b08 63 }
twighk 0:200635fa1b08 64
twighk 0:200635fa1b08 65 void motorencodetestline(){
twighk 0:200635fa1b08 66 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 67 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 0:200635fa1b08 68 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 69 const float speed = 0.2;
twighk 0:200635fa1b08 70 const float dspeed = 0.1;
twighk 0:200635fa1b08 71
twighk 0:200635fa1b08 72 mleft(speed); mright(speed);
twighk 0:200635fa1b08 73 while (true){
twighk 0:200635fa1b08 74 //left 27 cm = 113 -> 0.239 cm/pulse
twighk 0:200635fa1b08 75 //right 27 cm = 72 -> 0.375 cm/pulse
twighk 0:200635fa1b08 76 pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
twighk 0:200635fa1b08 77 if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
twighk 0:200635fa1b08 78 mright(speed - dspeed);
twighk 0:200635fa1b08 79 } else {
twighk 0:200635fa1b08 80 mright(speed + dspeed);
twighk 0:200635fa1b08 81 }
twighk 0:200635fa1b08 82 }
twighk 0:200635fa1b08 83
twighk 0:200635fa1b08 84 }
twighk 0:200635fa1b08 85
twighk 0:200635fa1b08 86 void motorencodetest(){
twighk 0:200635fa1b08 87 Encoder Eleft(p28, p27), Eright(p29, p30);
twighk 0:200635fa1b08 88 MainMotor mleft(p23,p24), mright(p22,p21);
twighk 0:200635fa1b08 89 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 90
twighk 0:200635fa1b08 91 const float speed = -0.3;
twighk 0:200635fa1b08 92 const int enc = -38;
twighk 0:200635fa1b08 93 while(true){
twighk 0:200635fa1b08 94 mleft(speed); mright(0);
twighk 0:200635fa1b08 95 while(Eleft.getPoint()>enc){
twighk 0:200635fa1b08 96 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 97 }
twighk 0:200635fa1b08 98 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 99 mleft(0); mright(speed);
twighk 0:200635fa1b08 100 while(Eright.getPoint()>enc){
twighk 0:200635fa1b08 101 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 102 }
twighk 0:200635fa1b08 103 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 104 }
twighk 0:200635fa1b08 105 }
twighk 0:200635fa1b08 106
twighk 0:200635fa1b08 107 void encodertest(){
twighk 0:200635fa1b08 108 Encoder E1(p28, p27);
twighk 0:200635fa1b08 109 Encoder E2(p29, p30);
twighk 0:200635fa1b08 110 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 111 while(1){
twighk 0:200635fa1b08 112 wait(0.1);
twighk 0:200635fa1b08 113 pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), E2.getPoint());
twighk 0:200635fa1b08 114 }
twighk 0:200635fa1b08 115
twighk 0:200635fa1b08 116 }
twighk 0:200635fa1b08 117 void motortest(){
twighk 0:200635fa1b08 118 MainMotor mright(p22,p21), mleft(p23,p24);
twighk 0:200635fa1b08 119 while(1) {
twighk 0:200635fa1b08 120 wait(1);
twighk 0:200635fa1b08 121 mleft(0.8); mright(0.8);
twighk 0:200635fa1b08 122 wait(1);
twighk 0:200635fa1b08 123 mleft(-0.2); mright(0.2);
twighk 0:200635fa1b08 124 wait(1);
twighk 0:200635fa1b08 125 mleft(0); mright(0);
twighk 0:200635fa1b08 126 }
twighk 0:200635fa1b08 127 }