Ryan Savitski / Mbed 2 deprecated ICRSEurobot14

2014 Eurobot fork

Dependencies:   mbed-rtos mbed QEI

main.cpp@7:4340355261f9, 2013-04-05 (annotated)

Committer:
madcowswe
Date:
Fri Apr 05 16:37:36 2013 +0000
Revision:
7:4340355261f9
Parent:
6:995b3679155f
Child:
10:1f0cf0182067
Cleaned up bullshit uneeded stuff

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
madcowswe 6:995b3679155f 5
madcowswe 6:995b3679155f 6
madcowswe 6:995b3679155f 7 /*
madcowswe 6:995b3679155f 8 PINOUT Sensors
madcowswe 6:995b3679155f 9 5: RF:SDI
madcowswe 6:995b3679155f 10 6 SDO
madcowswe 6:995b3679155f 11 7 SCK
madcowswe 6:995b3679155f 12 8 NCS
madcowswe 6:995b3679155f 13 9 NIRQ
madcowswe 6:995b3679155f 14 10-15 6 echo pins
madcowswe 6:995b3679155f 15 16 trig
madcowswe 6:995b3679155f 16 17 IRin
madcowswe 6:995b3679155f 17 18-20 unused
madcowswe 6:995b3679155f 18 21 stepper step
madcowswe 6:995b3679155f 19 22-27 unused
madcowswe 6:995b3679155f 20 28 Serial TX
madcowswe 6:995b3679155f 21 29-30 unused
madcowswe 6:995b3679155f 22
madcowswe 6:995b3679155f 23
madcowswe 6:995b3679155f 24 PINOUT Main
madcowswe 6:995b3679155f 25 5: Lower arm servo
madcowswe 6:995b3679155f 26 6: Upper arm servo
madcowswe 6:995b3679155f 27
madcowswe 6:995b3679155f 28 14: Serial RX
madcowswe 6:995b3679155f 29 15: Distance sensor
madcowswe 6:995b3679155f 30
madcowswe 6:995b3679155f 31 20: color sensor in
madcowswe 6:995b3679155f 32 21-24: Motors PWM IN 1-4
madcowswe 6:995b3679155f 33 25-26: Encoders
madcowswe 6:995b3679155f 34 27-28: Encoders
madcowswe 6:995b3679155f 35 29: Color sensor RED LED
madcowswe 6:995b3679155f 36 30: Color sensor BLUE LED
madcowswe 6:995b3679155f 37
madcowswe 6:995b3679155f 38 */
madcowswe 6:995b3679155f 39
madcowswe 6:995b3679155f 40 #define P_SERVO_LOWER_ARM p5
madcowswe 6:995b3679155f 41 #define P_SERVO_UPPER_ARM p6
madcowswe 6:995b3679155f 42
madcowswe 6:995b3679155f 43 #define P_SERIAL_RX p14
madcowswe 6:995b3679155f 44 #define P_DISTANCE_SENSOR p15
madcowswe 6:995b3679155f 45
madcowswe 6:995b3679155f 46 #define P_COLOR_SENSOR_IN p20
madcowswe 6:995b3679155f 47
madcowswe 6:995b3679155f 48 #define P_MOT_RIGHT_A p21
madcowswe 6:995b3679155f 49 #define P_MOT_RIGHT_B p22
madcowswe 6:995b3679155f 50 #define P_MOT_LEFT_A p23
madcowswe 6:995b3679155f 51 #define P_MOT_LEFT_B p24
madcowswe 6:995b3679155f 52
madcowswe 6:995b3679155f 53 #define P_ENC_RIGHT_A p28
madcowswe 6:995b3679155f 54 #define P_ENC_RIGHT_B p27
madcowswe 6:995b3679155f 55 #define P_ENC_LEFT_A p25
madcowswe 6:995b3679155f 56 #define P_ENC_LEFT_B p26
madcowswe 6:995b3679155f 57
madcowswe 6:995b3679155f 58 #define P_COLOR_SENSOR_RED p29
madcowswe 6:995b3679155f 59 #define P_COLOR_SENSOR_BLUE p30
madcowswe 6:995b3679155f 60
madcowswe 6:995b3679155f 61
madcowswe 6:995b3679155f 62
twighk 0:200635fa1b08 63 #include "mbed.h"
twighk 3:717de74f6ebd 64 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 65
twighk 4:1be0f6c6ceae 66 #include "Actuators/Arms/Arm.h"
twighk 1:8119211eae14 67 #include "Actuators/MainMotors/MainMotor.h"
twighk 1:8119211eae14 68 #include "Sensors/Encoders/Encoder.h"
twighk 4:1be0f6c6ceae 69 #include "Sensors/Colour/Colour.h"
twighk 0:200635fa1b08 70
twighk 2:45da48fab346 71
twighk 0:200635fa1b08 72
twighk 0:200635fa1b08 73 void motortest();
twighk 0:200635fa1b08 74 void encodertest();
twighk 0:200635fa1b08 75 void motorencodetest();
twighk 0:200635fa1b08 76 void motorencodetestline();
twighk 0:200635fa1b08 77 void motorsandservostest();
twighk 1:8119211eae14 78 void armtest();
twighk 2:45da48fab346 79 void motortestline();
twighk 3:717de74f6ebd 80 void ledtest();
twighk 3:717de74f6ebd 81 void phototransistortest();
twighk 3:717de74f6ebd 82 void ledphototransistortest();
twighk 3:717de74f6ebd 83 void colourtest();
twighk 0:200635fa1b08 84
twighk 0:200635fa1b08 85 int main() {
twighk 2:45da48fab346 86
twighk 3:717de74f6ebd 87 /*****************
twighk 3:717de74f6ebd 88 * Test Code *
twighk 3:717de74f6ebd 89 *****************/
twighk 0:200635fa1b08 90 //motortest();
twighk 0:200635fa1b08 91 //encodertest();
madcowswe 7:4340355261f9 92 //motorencodetest();
twighk 1:8119211eae14 93 //motorencodetestline();
twighk 0:200635fa1b08 94 //motorsandservostest();
twighk 3:717de74f6ebd 95 //armtest();
twighk 2:45da48fab346 96 //motortestline();
madcowswe 7:4340355261f9 97 ledtest();
twighk 3:717de74f6ebd 98 //phototransistortest();
twighk 3:717de74f6ebd 99 //ledphototransistortest();
madcowswe 5:56a5fdd373c9 100 //colourtest(); // Red SnR too low
twighk 3:717de74f6ebd 101 }
twighk 3:717de74f6ebd 102
twighk 3:717de74f6ebd 103 void colourtest(){
madcowswe 7:4340355261f9 104 Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 105 c.Calibrate();
twighk 3:717de74f6ebd 106 while(true){
twighk 3:717de74f6ebd 107 wait(0.1);
twighk 3:717de74f6ebd 108 ColourEnum ce = c.getColour();
twighk 3:717de74f6ebd 109 switch(ce){
twighk 3:717de74f6ebd 110 case BLUE :
twighk 3:717de74f6ebd 111 pc.printf("BLUE\n\r");
twighk 3:717de74f6ebd 112 break;
twighk 3:717de74f6ebd 113 case RED:
twighk 3:717de74f6ebd 114 pc.printf("RED\n\r");
twighk 3:717de74f6ebd 115 break;
twighk 3:717de74f6ebd 116 case WHITE:
twighk 3:717de74f6ebd 117 pc.printf("WHITE\n\r");
twighk 3:717de74f6ebd 118 break;
twighk 3:717de74f6ebd 119 case INCONCLUSIVE:
twighk 3:717de74f6ebd 120 pc.printf("INCONCLUSIVE\n\r");
twighk 3:717de74f6ebd 121 break;
twighk 3:717de74f6ebd 122 default:
twighk 3:717de74f6ebd 123 pc.printf("BUG\n\r");
twighk 3:717de74f6ebd 124 }
twighk 2:45da48fab346 125 }
twighk 0:200635fa1b08 126
twighk 3:717de74f6ebd 127 }
twighk 3:717de74f6ebd 128
twighk 3:717de74f6ebd 129
twighk 3:717de74f6ebd 130 void ledphototransistortest(){
madcowswe 7:4340355261f9 131 DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
madcowswe 7:4340355261f9 132 AnalogIn pt(P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 133 Serial pc(USBTX, USBRX);
twighk 3:717de74f6ebd 134
twighk 3:717de74f6ebd 135 while(true){
madcowswe 7:4340355261f9 136 blue = 1; red = 0;
twighk 3:717de74f6ebd 137 for(int i = 0; i != 5; i++){
twighk 3:717de74f6ebd 138 wait(0.1);
twighk 3:717de74f6ebd 139 pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
twighk 3:717de74f6ebd 140 }
madcowswe 7:4340355261f9 141 blue = 0; red = 1;
twighk 3:717de74f6ebd 142 for(int i = 0; i != 5; i++){
twighk 3:717de74f6ebd 143 wait(0.1);
twighk 3:717de74f6ebd 144 pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
twighk 3:717de74f6ebd 145 }
twighk 3:717de74f6ebd 146 }
twighk 3:717de74f6ebd 147 }
twighk 3:717de74f6ebd 148
twighk 3:717de74f6ebd 149 void phototransistortest(){
madcowswe 7:4340355261f9 150 AnalogIn pt(P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 151 while(true){
twighk 3:717de74f6ebd 152 wait(0.1);
twighk 3:717de74f6ebd 153 pc.printf("Phototransistor Analog is: %f \n\r", pt.read());
twighk 3:717de74f6ebd 154 }
twighk 3:717de74f6ebd 155
twighk 3:717de74f6ebd 156 }
twighk 3:717de74f6ebd 157
twighk 3:717de74f6ebd 158 void ledtest(){
madcowswe 7:4340355261f9 159 DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
twighk 3:717de74f6ebd 160 while(true){
madcowswe 7:4340355261f9 161 blue = 1; red = 0;
twighk 3:717de74f6ebd 162 wait(0.2);
madcowswe 7:4340355261f9 163 blue = 0; red = 1;
twighk 3:717de74f6ebd 164 wait(0.2);
twighk 3:717de74f6ebd 165
twighk 3:717de74f6ebd 166 }
twighk 3:717de74f6ebd 167 }
twighk 3:717de74f6ebd 168
twighk 1:8119211eae14 169 void armtest(){
twighk 3:717de74f6ebd 170 Arm white(p26), black(p25, false, 0.0005, 180);
twighk 3:717de74f6ebd 171 while(true){
twighk 1:8119211eae14 172 white(0);
twighk 1:8119211eae14 173 black(0);
twighk 1:8119211eae14 174 wait(1);
twighk 1:8119211eae14 175 white(1);
twighk 1:8119211eae14 176 black(1);
twighk 1:8119211eae14 177 wait(1);
twighk 1:8119211eae14 178 }
twighk 1:8119211eae14 179 }
twighk 1:8119211eae14 180
twighk 1:8119211eae14 181
twighk 0:200635fa1b08 182 void motorsandservostest(){
twighk 0:200635fa1b08 183 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 184 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 1:8119211eae14 185 Arm sTop(p25), sBottom(p26);
twighk 4:1be0f6c6ceae 186 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 187 const float speed = 0.0;
twighk 0:200635fa1b08 188 const float dspeed = 0.0;
twighk 0:200635fa1b08 189
twighk 0:200635fa1b08 190 Timer servoTimer;
twighk 0:200635fa1b08 191 mleft(speed); mright(speed);
twighk 0:200635fa1b08 192 servoTimer.start();
twighk 0:200635fa1b08 193 while (true){
twighk 0:200635fa1b08 194 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 195 if (Eleft.getPoint() < Eright.getPoint()){
twighk 0:200635fa1b08 196 mleft(speed);
twighk 0:200635fa1b08 197 mright(speed - dspeed);
twighk 0:200635fa1b08 198 } else {
twighk 0:200635fa1b08 199 mright(speed);
twighk 0:200635fa1b08 200 mleft(speed - dspeed);
twighk 0:200635fa1b08 201 }
twighk 0:200635fa1b08 202 if (servoTimer.read() < 1){
twighk 0:200635fa1b08 203 sTop.clockwise();
twighk 0:200635fa1b08 204 } else if (servoTimer.read() < 4) {
twighk 2:45da48fab346 205 sTop.halt();
twighk 0:200635fa1b08 206 } else if (servoTimer.read() < 5) {
twighk 0:200635fa1b08 207 sBottom.anticlockwise();
twighk 0:200635fa1b08 208 //Led=1;
twighk 0:200635fa1b08 209 } else if (servoTimer.read() < 6) {
twighk 0:200635fa1b08 210 sBottom.clockwise();
twighk 0:200635fa1b08 211 //Led=0;
twighk 0:200635fa1b08 212 } else if (servoTimer.read() < 7) {
twighk 2:45da48fab346 213 sBottom.halt();
twighk 0:200635fa1b08 214 }else {
twighk 0:200635fa1b08 215 sTop.anticlockwise();
twighk 0:200635fa1b08 216 }
twighk 0:200635fa1b08 217 if (servoTimer.read() >= 9) servoTimer.reset();
twighk 0:200635fa1b08 218 }
twighk 0:200635fa1b08 219 }
twighk 0:200635fa1b08 220
twighk 2:45da48fab346 221 void motortestline(){
twighk 2:45da48fab346 222 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 2:45da48fab346 223 const float speed = 0.2;
twighk 2:45da48fab346 224 mleft(speed); mright(speed);
twighk 2:45da48fab346 225 while(true) wait(1);
twighk 2:45da48fab346 226 }
twighk 2:45da48fab346 227
twighk 0:200635fa1b08 228 void motorencodetestline(){
twighk 0:200635fa1b08 229 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 230 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 4:1be0f6c6ceae 231 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 232 const float speed = 0.2;
twighk 0:200635fa1b08 233 const float dspeed = 0.1;
twighk 0:200635fa1b08 234
twighk 0:200635fa1b08 235 mleft(speed); mright(speed);
twighk 0:200635fa1b08 236 while (true){
twighk 0:200635fa1b08 237 //left 27 cm = 113 -> 0.239 cm/pulse
twighk 0:200635fa1b08 238 //right 27 cm = 72 -> 0.375 cm/pulse
twighk 0:200635fa1b08 239 pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
twighk 0:200635fa1b08 240 if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
twighk 0:200635fa1b08 241 mright(speed - dspeed);
twighk 0:200635fa1b08 242 } else {
twighk 0:200635fa1b08 243 mright(speed + dspeed);
twighk 0:200635fa1b08 244 }
twighk 0:200635fa1b08 245 }
twighk 0:200635fa1b08 246
twighk 0:200635fa1b08 247 }
twighk 0:200635fa1b08 248
twighk 0:200635fa1b08 249 void motorencodetest(){
madcowswe 7:4340355261f9 250 Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 7:4340355261f9 251 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
twighk 0:200635fa1b08 252 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 253
twighk 0:200635fa1b08 254 const float speed = -0.3;
twighk 0:200635fa1b08 255 const int enc = -38;
twighk 0:200635fa1b08 256 while(true){
twighk 0:200635fa1b08 257 mleft(speed); mright(0);
twighk 0:200635fa1b08 258 while(Eleft.getPoint()>enc){
twighk 0:200635fa1b08 259 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 260 }
twighk 0:200635fa1b08 261 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 262 mleft(0); mright(speed);
twighk 0:200635fa1b08 263 while(Eright.getPoint()>enc){
twighk 0:200635fa1b08 264 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 265 }
twighk 0:200635fa1b08 266 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 267 }
twighk 0:200635fa1b08 268 }
twighk 0:200635fa1b08 269
twighk 0:200635fa1b08 270 void encodertest(){
twighk 0:200635fa1b08 271 Encoder E1(p28, p27);
twighk 0:200635fa1b08 272 Encoder E2(p29, p30);
twighk 0:200635fa1b08 273 Serial pc(USBTX, USBRX);
twighk 3:717de74f6ebd 274 while(true){
twighk 0:200635fa1b08 275 wait(0.1);
twighk 0:200635fa1b08 276 pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), E2.getPoint());
twighk 0:200635fa1b08 277 }
twighk 0:200635fa1b08 278
twighk 0:200635fa1b08 279 }
twighk 0:200635fa1b08 280 void motortest(){
twighk 0:200635fa1b08 281 MainMotor mright(p22,p21), mleft(p23,p24);
twighk 3:717de74f6ebd 282 while(true) {
twighk 0:200635fa1b08 283 wait(1);
twighk 0:200635fa1b08 284 mleft(0.8); mright(0.8);
twighk 0:200635fa1b08 285 wait(1);
twighk 0:200635fa1b08 286 mleft(-0.2); mright(0.2);
twighk 0:200635fa1b08 287 wait(1);
twighk 0:200635fa1b08 288 mleft(0); mright(0);
twighk 0:200635fa1b08 289 }
twighk 0:200635fa1b08 290 }