Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

main.cpp@2:45da48fab346, 2013-03-29 (annotated)

Committer:
twighk
Date:
Fri Mar 29 20:09:21 2013 +0000
Revision:
2:45da48fab346
Parent:
1:8119211eae14
Child:
3:717de74f6ebd
Emergency Stop Button, Derive all actuator classes from the base class, and implement a virtual halt function that releases power from them

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