Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

main.cpp@1:8119211eae14, 2013-03-29 (annotated)

Committer:
twighk
Date:
Fri Mar 29 16:28:56 2013 +0000
Revision:
1:8119211eae14
Parent:
0:200635fa1b08
Child:
2:45da48fab346
Arms / servos

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