Cooper Liu / Mbed 2 deprecated ICRSEurobot13

Colour sensors calibrated

Dependencies:   mbed-rtos mbed Servo QEI

Fork of ICRSEurobot13 by Thomas Branch

main.cpp@5:56a5fdd373c9, 2013-04-04 (annotated)

Committer:
madcowswe
Date:
Thu Apr 04 14:44:14 2013 +0000
Revision:
5:56a5fdd373c9
Parent:
4:1be0f6c6ceae
Child:
6:995b3679155f
Child:
8:69bdf20cb525
Removed e stop button code, as power will be used through the button instead.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
twighk 3:717de74f6ebd 1 #pragma Otime // Compiler Optimisations
twighk 0:200635fa1b08 2
twighk 0:200635fa1b08 3 // Eurobot13 main.cpp
twighk 0:200635fa1b08 4
twighk 0:200635fa1b08 5 #include "mbed.h"
twighk 3:717de74f6ebd 6 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 7
twighk 4:1be0f6c6ceae 8 #include "Actuators/Arms/Arm.h"
twighk 1:8119211eae14 9 #include "Actuators/MainMotors/MainMotor.h"
twighk 1:8119211eae14 10 #include "Sensors/Encoders/Encoder.h"
twighk 4:1be0f6c6ceae 11 #include "Sensors/Colour/Colour.h"
twighk 3:717de74f6ebd 12 #include "Sensors/Colour/Led.h"
twighk 3:717de74f6ebd 13 #include "Sensors/Colour/Phototransistor.h"
twighk 0:200635fa1b08 14
twighk 2:45da48fab346 15
twighk 0:200635fa1b08 16
twighk 0:200635fa1b08 17 void motortest();
twighk 0:200635fa1b08 18 void encodertest();
twighk 0:200635fa1b08 19 void motorencodetest();
twighk 0:200635fa1b08 20 void motorencodetestline();
twighk 0:200635fa1b08 21 void motorsandservostest();
twighk 1:8119211eae14 22 void armtest();
twighk 2:45da48fab346 23 void motortestline();
twighk 3:717de74f6ebd 24 void ledtest();
twighk 3:717de74f6ebd 25 void phototransistortest();
twighk 3:717de74f6ebd 26 void ledphototransistortest();
twighk 3:717de74f6ebd 27 void colourtest();
twighk 0:200635fa1b08 28
twighk 0:200635fa1b08 29 int main() {
twighk 2:45da48fab346 30
twighk 3:717de74f6ebd 31 /*****************
twighk 3:717de74f6ebd 32 * Test Code *
twighk 3:717de74f6ebd 33 *****************/
twighk 0:200635fa1b08 34 //motortest();
twighk 0:200635fa1b08 35 //encodertest();
madcowswe 5:56a5fdd373c9 36 motorencodetest();
twighk 1:8119211eae14 37 //motorencodetestline();
twighk 0:200635fa1b08 38 //motorsandservostest();
twighk 3:717de74f6ebd 39 //armtest();
twighk 2:45da48fab346 40 //motortestline();
twighk 3:717de74f6ebd 41 //ledtest();
twighk 3:717de74f6ebd 42 //phototransistortest();
twighk 3:717de74f6ebd 43 //ledphototransistortest();
madcowswe 5:56a5fdd373c9 44 //colourtest(); // Red SnR too low
twighk 3:717de74f6ebd 45 }
twighk 3:717de74f6ebd 46
twighk 3:717de74f6ebd 47 void colourtest(){
twighk 3:717de74f6ebd 48 Colour c(p9,p10,p20);
twighk 3:717de74f6ebd 49 c.Calibrate();
twighk 3:717de74f6ebd 50 while(true){
twighk 3:717de74f6ebd 51 wait(0.1);
twighk 3:717de74f6ebd 52 ColourEnum ce = c.getColour();
twighk 3:717de74f6ebd 53 switch(ce){
twighk 3:717de74f6ebd 54 case BLUE :
twighk 3:717de74f6ebd 55 pc.printf("BLUE\n\r");
twighk 3:717de74f6ebd 56 break;
twighk 3:717de74f6ebd 57 case RED:
twighk 3:717de74f6ebd 58 pc.printf("RED\n\r");
twighk 3:717de74f6ebd 59 break;
twighk 3:717de74f6ebd 60 case WHITE:
twighk 3:717de74f6ebd 61 pc.printf("WHITE\n\r");
twighk 3:717de74f6ebd 62 break;
twighk 3:717de74f6ebd 63 case INCONCLUSIVE:
twighk 3:717de74f6ebd 64 pc.printf("INCONCLUSIVE\n\r");
twighk 3:717de74f6ebd 65 break;
twighk 3:717de74f6ebd 66 default:
twighk 3:717de74f6ebd 67 pc.printf("BUG\n\r");
twighk 3:717de74f6ebd 68 }
twighk 2:45da48fab346 69 }
twighk 0:200635fa1b08 70
twighk 3:717de74f6ebd 71 }
twighk 3:717de74f6ebd 72
twighk 3:717de74f6ebd 73
twighk 3:717de74f6ebd 74 void ledphototransistortest(){
twighk 3:717de74f6ebd 75 Led blue(p9), red(p10);
twighk 3:717de74f6ebd 76 Phototransistor pt(p20);
twighk 3:717de74f6ebd 77 Serial pc(USBTX, USBRX);
twighk 3:717de74f6ebd 78
twighk 3:717de74f6ebd 79 while(true){
twighk 3:717de74f6ebd 80 blue.on();red.off();
twighk 3:717de74f6ebd 81 for(int i = 0; i != 5; i++){
twighk 3:717de74f6ebd 82 wait(0.1);
twighk 3:717de74f6ebd 83 pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
twighk 3:717de74f6ebd 84 }
twighk 3:717de74f6ebd 85 blue.off();red.on();
twighk 3:717de74f6ebd 86 for(int i = 0; i != 5; i++){
twighk 3:717de74f6ebd 87 wait(0.1);
twighk 3:717de74f6ebd 88 pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
twighk 3:717de74f6ebd 89 }
twighk 3:717de74f6ebd 90 }
twighk 3:717de74f6ebd 91 }
twighk 3:717de74f6ebd 92
twighk 3:717de74f6ebd 93 void phototransistortest(){
twighk 3:717de74f6ebd 94 Phototransistor pt(p20);
twighk 3:717de74f6ebd 95 while(true){
twighk 3:717de74f6ebd 96 wait(0.1);
twighk 3:717de74f6ebd 97 pc.printf("Phototransistor Analog is: %f \n\r", pt.read());
twighk 3:717de74f6ebd 98 }
twighk 3:717de74f6ebd 99
twighk 3:717de74f6ebd 100 }
twighk 3:717de74f6ebd 101
twighk 3:717de74f6ebd 102 void ledtest(){
twighk 3:717de74f6ebd 103 Led blue(p9), red(p10);
twighk 3:717de74f6ebd 104 while(true){
twighk 3:717de74f6ebd 105 blue.on();red.off();
twighk 3:717de74f6ebd 106 wait(0.2);
twighk 3:717de74f6ebd 107 blue.off();red.on();
twighk 3:717de74f6ebd 108 wait(0.2);
twighk 3:717de74f6ebd 109
twighk 3:717de74f6ebd 110 }
twighk 3:717de74f6ebd 111 }
twighk 3:717de74f6ebd 112
twighk 1:8119211eae14 113 void armtest(){
twighk 3:717de74f6ebd 114 Arm white(p26), black(p25, false, 0.0005, 180);
twighk 3:717de74f6ebd 115 while(true){
twighk 1:8119211eae14 116 white(0);
twighk 1:8119211eae14 117 black(0);
twighk 1:8119211eae14 118 wait(1);
twighk 1:8119211eae14 119 white(1);
twighk 1:8119211eae14 120 black(1);
twighk 1:8119211eae14 121 wait(1);
twighk 1:8119211eae14 122 }
twighk 1:8119211eae14 123 }
twighk 1:8119211eae14 124
twighk 1:8119211eae14 125
twighk 0:200635fa1b08 126 void motorsandservostest(){
twighk 0:200635fa1b08 127 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 128 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 1:8119211eae14 129 Arm sTop(p25), sBottom(p26);
twighk 4:1be0f6c6ceae 130 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 131 const float speed = 0.0;
twighk 0:200635fa1b08 132 const float dspeed = 0.0;
twighk 0:200635fa1b08 133
twighk 0:200635fa1b08 134 Timer servoTimer;
twighk 0:200635fa1b08 135 mleft(speed); mright(speed);
twighk 0:200635fa1b08 136 servoTimer.start();
twighk 0:200635fa1b08 137 while (true){
twighk 0:200635fa1b08 138 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 139 if (Eleft.getPoint() < Eright.getPoint()){
twighk 0:200635fa1b08 140 mleft(speed);
twighk 0:200635fa1b08 141 mright(speed - dspeed);
twighk 0:200635fa1b08 142 } else {
twighk 0:200635fa1b08 143 mright(speed);
twighk 0:200635fa1b08 144 mleft(speed - dspeed);
twighk 0:200635fa1b08 145 }
twighk 0:200635fa1b08 146 if (servoTimer.read() < 1){
twighk 0:200635fa1b08 147 sTop.clockwise();
twighk 0:200635fa1b08 148 } else if (servoTimer.read() < 4) {
twighk 2:45da48fab346 149 sTop.halt();
twighk 0:200635fa1b08 150 } else if (servoTimer.read() < 5) {
twighk 0:200635fa1b08 151 sBottom.anticlockwise();
twighk 0:200635fa1b08 152 //Led=1;
twighk 0:200635fa1b08 153 } else if (servoTimer.read() < 6) {
twighk 0:200635fa1b08 154 sBottom.clockwise();
twighk 0:200635fa1b08 155 //Led=0;
twighk 0:200635fa1b08 156 } else if (servoTimer.read() < 7) {
twighk 2:45da48fab346 157 sBottom.halt();
twighk 0:200635fa1b08 158 }else {
twighk 0:200635fa1b08 159 sTop.anticlockwise();
twighk 0:200635fa1b08 160 }
twighk 0:200635fa1b08 161 if (servoTimer.read() >= 9) servoTimer.reset();
twighk 0:200635fa1b08 162 }
twighk 0:200635fa1b08 163 }
twighk 0:200635fa1b08 164
twighk 2:45da48fab346 165 void motortestline(){
twighk 2:45da48fab346 166 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 2:45da48fab346 167 const float speed = 0.2;
twighk 2:45da48fab346 168 mleft(speed); mright(speed);
twighk 2:45da48fab346 169 while(true) wait(1);
twighk 2:45da48fab346 170 }
twighk 2:45da48fab346 171
twighk 0:200635fa1b08 172 void motorencodetestline(){
twighk 0:200635fa1b08 173 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 174 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 4:1be0f6c6ceae 175 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 176 const float speed = 0.2;
twighk 0:200635fa1b08 177 const float dspeed = 0.1;
twighk 0:200635fa1b08 178
twighk 0:200635fa1b08 179 mleft(speed); mright(speed);
twighk 0:200635fa1b08 180 while (true){
twighk 0:200635fa1b08 181 //left 27 cm = 113 -> 0.239 cm/pulse
twighk 0:200635fa1b08 182 //right 27 cm = 72 -> 0.375 cm/pulse
twighk 0:200635fa1b08 183 pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
twighk 0:200635fa1b08 184 if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
twighk 0:200635fa1b08 185 mright(speed - dspeed);
twighk 0:200635fa1b08 186 } else {
twighk 0:200635fa1b08 187 mright(speed + dspeed);
twighk 0:200635fa1b08 188 }
twighk 0:200635fa1b08 189 }
twighk 0:200635fa1b08 190
twighk 0:200635fa1b08 191 }
twighk 0:200635fa1b08 192
twighk 0:200635fa1b08 193 void motorencodetest(){
twighk 0:200635fa1b08 194 Encoder Eleft(p28, p27), Eright(p29, p30);
twighk 0:200635fa1b08 195 MainMotor mleft(p23,p24), mright(p22,p21);
twighk 0:200635fa1b08 196 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 197
twighk 0:200635fa1b08 198 const float speed = -0.3;
twighk 0:200635fa1b08 199 const int enc = -38;
twighk 0:200635fa1b08 200 while(true){
twighk 0:200635fa1b08 201 mleft(speed); mright(0);
twighk 0:200635fa1b08 202 while(Eleft.getPoint()>enc){
twighk 0:200635fa1b08 203 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 204 }
twighk 0:200635fa1b08 205 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 206 mleft(0); mright(speed);
twighk 0:200635fa1b08 207 while(Eright.getPoint()>enc){
twighk 0:200635fa1b08 208 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 209 }
twighk 0:200635fa1b08 210 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 211 }
twighk 0:200635fa1b08 212 }
twighk 0:200635fa1b08 213
twighk 0:200635fa1b08 214 void encodertest(){
twighk 0:200635fa1b08 215 Encoder E1(p28, p27);
twighk 0:200635fa1b08 216 Encoder E2(p29, p30);
twighk 0:200635fa1b08 217 Serial pc(USBTX, USBRX);
twighk 3:717de74f6ebd 218 while(true){
twighk 0:200635fa1b08 219 wait(0.1);
twighk 0:200635fa1b08 220 pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), E2.getPoint());
twighk 0:200635fa1b08 221 }
twighk 0:200635fa1b08 222
twighk 0:200635fa1b08 223 }
twighk 0:200635fa1b08 224 void motortest(){
twighk 0:200635fa1b08 225 MainMotor mright(p22,p21), mleft(p23,p24);
twighk 3:717de74f6ebd 226 while(true) {
twighk 0:200635fa1b08 227 wait(1);
twighk 0:200635fa1b08 228 mleft(0.8); mright(0.8);
twighk 0:200635fa1b08 229 wait(1);
twighk 0:200635fa1b08 230 mleft(-0.2); mright(0.2);
twighk 0:200635fa1b08 231 wait(1);
twighk 0:200635fa1b08 232 mleft(0); mright(0);
twighk 0:200635fa1b08 233 }
twighk 0:200635fa1b08 234 }