Thomas Branch / Mbed 2 deprecated ICRSEurobot13

ICRS Eurobot 2013

Dependencies:   mbed mbed-rtos Servo QEI

main.cpp@19:4b993a9a156e, 2013-04-07 (annotated)

Committer:
madcowswe
Date:
Sun Apr 07 19:26:07 2013 +0000
Revision:
19:4b993a9a156e
Parent:
17:6263e90bf3ba
Child:
20:70d651156779
Kalman init almost ready for testing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
twighk 8:69bdf20cb525 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 */
twighk 0:200635fa1b08 39 #include "mbed.h"
twighk 12:d4b5851742a3 40 #include "rtos.h"
madcowswe 19:4b993a9a156e 41 #include "globals.h"
twighk 3:717de74f6ebd 42 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 43
twighk 11:bbddc908c78c 44 const PinName P_SERVO_LOWER_ARM = p5;
twighk 11:bbddc908c78c 45 const PinName P_SERVO_UPPER_ARM = p6;
twighk 11:bbddc908c78c 46
twighk 11:bbddc908c78c 47 const PinName P_SERIAL_RX = p14;
twighk 11:bbddc908c78c 48 const PinName P_DISTANCE_SENSOR = p15;
twighk 11:bbddc908c78c 49
twighk 11:bbddc908c78c 50 const PinName P_COLOR_SENSOR_IN = p20;
twighk 11:bbddc908c78c 51
twighk 11:bbddc908c78c 52 const PinName P_MOT_RIGHT_A = p21;
twighk 11:bbddc908c78c 53 const PinName P_MOT_RIGHT_B = p22;
twighk 11:bbddc908c78c 54 const PinName P_MOT_LEFT_A = p23;
twighk 11:bbddc908c78c 55 const PinName P_MOT_LEFT_B = p24;
twighk 11:bbddc908c78c 56
twighk 11:bbddc908c78c 57 const PinName P_ENC_RIGHT_A = p28;
twighk 11:bbddc908c78c 58 const PinName P_ENC_RIGHT_B = p27;
twighk 11:bbddc908c78c 59 const PinName P_ENC_LEFT_A = p25;
twighk 11:bbddc908c78c 60 const PinName P_ENC_LEFT_B = p26;
twighk 11:bbddc908c78c 61
twighk 11:bbddc908c78c 62 const PinName P_COLOR_SENSOR_RED = p29;
twighk 11:bbddc908c78c 63 const PinName P_COLOR_SENSOR_BLUE = p30;
twighk 11:bbddc908c78c 64
madcowswe 17:6263e90bf3ba 65 pos beaconpos[] = {{0,0}, {0,2}, {3,1}};
madcowswe 17:6263e90bf3ba 66
twighk 4:1be0f6c6ceae 67 #include "Actuators/Arms/Arm.h"
twighk 1:8119211eae14 68 #include "Actuators/MainMotors/MainMotor.h"
twighk 1:8119211eae14 69 #include "Sensors/Encoders/Encoder.h"
twighk 4:1be0f6c6ceae 70 #include "Sensors/Colour/Colour.h"
twighk 8:69bdf20cb525 71 #include "Sensors/CakeSensor/CakeSensor.h"
twighk 12:d4b5851742a3 72 #include "Processes/Printing/Printing.h"
madcowswe 13:76c9915db820 73 #include <algorithm>
twighk 0:200635fa1b08 74
twighk 0:200635fa1b08 75 void motortest();
twighk 0:200635fa1b08 76 void encodertest();
twighk 0:200635fa1b08 77 void motorencodetest();
twighk 0:200635fa1b08 78 void motorencodetestline();
twighk 0:200635fa1b08 79 void motorsandservostest();
twighk 1:8119211eae14 80 void armtest();
twighk 2:45da48fab346 81 void motortestline();
twighk 3:717de74f6ebd 82 void ledtest();
twighk 3:717de74f6ebd 83 void phototransistortest();
twighk 3:717de74f6ebd 84 void ledphototransistortest();
twighk 3:717de74f6ebd 85 void colourtest();
twighk 8:69bdf20cb525 86 void cakesensortest();
twighk 12:d4b5851742a3 87 void printingtestthread(void const*);
twighk 12:d4b5851742a3 88 void printingtestthread2(void const*);
madcowswe 13:76c9915db820 89 void feedbacktest();
twighk 0:200635fa1b08 90
twighk 0:200635fa1b08 91 int main() {
twighk 2:45da48fab346 92
twighk 3:717de74f6ebd 93 /*****************
twighk 3:717de74f6ebd 94 * Test Code *
twighk 3:717de74f6ebd 95 *****************/
twighk 0:200635fa1b08 96 //motortest();
twighk 0:200635fa1b08 97 //encodertest();
twighk 8:69bdf20cb525 98 //motorencodetest();
twighk 1:8119211eae14 99 //motorencodetestline();
twighk 0:200635fa1b08 100 //motorsandservostest();
twighk 3:717de74f6ebd 101 //armtest();
twighk 2:45da48fab346 102 //motortestline();
twighk 11:bbddc908c78c 103 //ledtest();
twighk 3:717de74f6ebd 104 //phototransistortest();
madcowswe 13:76c9915db820 105 //ledphototransistortest();
madcowswe 5:56a5fdd373c9 106 //colourtest(); // Red SnR too low
madcowswe 13:76c9915db820 107 //cakesensortest();
madcowswe 15:9c5aaeda36dc 108 feedbacktest();
madcowswe 15:9c5aaeda36dc 109
madcowswe 15:9c5aaeda36dc 110 /*
twighk 12:d4b5851742a3 111 DigitalOut l1(LED1);
twighk 12:d4b5851742a3 112 Thread p(printingThread, NULL, osPriorityNormal, 2048);
twighk 12:d4b5851742a3 113 l1=1;
twighk 12:d4b5851742a3 114 Thread a(printingtestthread, NULL, osPriorityNormal, 1024);
twighk 12:d4b5851742a3 115 Thread b(printingtestthread2, NULL, osPriorityNormal, 1024);
twighk 12:d4b5851742a3 116 Thread::wait(osWaitForever);
madcowswe 15:9c5aaeda36dc 117 */
madcowswe 16:52250d8d8fce 118
madcowswe 16:52250d8d8fce 119 //Kalman test threads
madcowswe 16:52250d8d8fce 120 //Ticker predictticker;
madcowswe 16:52250d8d8fce 121 //predictthread(predictloopwrapper, this, osPriorityNormal, 512)
madcowswe 16:52250d8d8fce 122 //updatethread(updateloopwrapper, this, osPriorityNormal, 512)
madcowswe 16:52250d8d8fce 123 //predictticker( SIGTICKARGS(predictthread, 0x1) ),
madcowswe 13:76c9915db820 124 }
madcowswe 13:76c9915db820 125
twighk 12:d4b5851742a3 126 #include <cstdlib>
twighk 12:d4b5851742a3 127 using namespace std;
twighk 12:d4b5851742a3 128
twighk 12:d4b5851742a3 129 void printingtestthread(void const*){
twighk 12:d4b5851742a3 130 const char ID = 1;
twighk 12:d4b5851742a3 131 float buffer[3] = {ID};
twighk 12:d4b5851742a3 132 registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
twighk 12:d4b5851742a3 133 while (true){
twighk 12:d4b5851742a3 134 for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
twighk 12:d4b5851742a3 135 buffer[i] =ID ;
twighk 12:d4b5851742a3 136 }
twighk 12:d4b5851742a3 137 updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
twighk 12:d4b5851742a3 138 Thread::wait(200);
twighk 12:d4b5851742a3 139 }
twighk 12:d4b5851742a3 140 }
madcowswe 14:c638d4b9ee94 141
twighk 12:d4b5851742a3 142 void printingtestthread2(void const*){
twighk 12:d4b5851742a3 143 const char ID = 2;
twighk 12:d4b5851742a3 144 float buffer[5] = {ID};
twighk 12:d4b5851742a3 145 registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
twighk 12:d4b5851742a3 146 while (true){
twighk 12:d4b5851742a3 147 for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
twighk 12:d4b5851742a3 148 buffer[i] = ID;
twighk 12:d4b5851742a3 149 }
twighk 12:d4b5851742a3 150 updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
twighk 12:d4b5851742a3 151 Thread::wait(500);
twighk 12:d4b5851742a3 152 }
twighk 8:69bdf20cb525 153 }
twighk 8:69bdf20cb525 154
madcowswe 14:c638d4b9ee94 155
madcowswe 13:76c9915db820 156 void feedbacktest(){
madcowswe 13:76c9915db820 157 Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 13:76c9915db820 158 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
madcowswe 13:76c9915db820 159
madcowswe 15:9c5aaeda36dc 160 float Pgain = -0.005;
madcowswe 15:9c5aaeda36dc 161 float fwdspeed = -400/3.0f;
madcowswe 13:76c9915db820 162 Timer timer;
madcowswe 13:76c9915db820 163 timer.start();
madcowswe 13:76c9915db820 164
madcowswe 13:76c9915db820 165 while(true){
madcowswe 13:76c9915db820 166 float expecdist = fwdspeed * timer.read();
madcowswe 15:9c5aaeda36dc 167 float errleft = Eleft.getPoint() - (expecdist*1.05);
madcowswe 13:76c9915db820 168 float errright = Eright.getPoint() - expecdist;
madcowswe 13:76c9915db820 169
madcowswe 13:76c9915db820 170 mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
madcowswe 13:76c9915db820 171 mright(max(min(errright*Pgain, 0.4f), -0.4f));
madcowswe 13:76c9915db820 172 }
twighk 8:69bdf20cb525 173 }
twighk 8:69bdf20cb525 174
twighk 8:69bdf20cb525 175 void cakesensortest(){
twighk 8:69bdf20cb525 176 wait(1);
twighk 8:69bdf20cb525 177 pc.printf("cakesensortest");
twighk 8:69bdf20cb525 178
twighk 11:bbddc908c78c 179 CakeSensor cs(P_COLOR_SENSOR_IN);
twighk 8:69bdf20cb525 180 while(true){
twighk 8:69bdf20cb525 181 wait(0.1);
twighk 8:69bdf20cb525 182 pc.printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
twighk 8:69bdf20cb525 183 }
twighk 3:717de74f6ebd 184 }
twighk 3:717de74f6ebd 185
twighk 3:717de74f6ebd 186 void colourtest(){
madcowswe 7:4340355261f9 187 Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 188 c.Calibrate();
twighk 3:717de74f6ebd 189 while(true){
twighk 3:717de74f6ebd 190 wait(0.1);
twighk 3:717de74f6ebd 191 ColourEnum ce = c.getColour();
twighk 3:717de74f6ebd 192 switch(ce){
twighk 3:717de74f6ebd 193 case BLUE :
twighk 3:717de74f6ebd 194 pc.printf("BLUE\n\r");
twighk 3:717de74f6ebd 195 break;
twighk 3:717de74f6ebd 196 case RED:
twighk 3:717de74f6ebd 197 pc.printf("RED\n\r");
twighk 3:717de74f6ebd 198 break;
twighk 3:717de74f6ebd 199 case WHITE:
twighk 3:717de74f6ebd 200 pc.printf("WHITE\n\r");
twighk 3:717de74f6ebd 201 break;
twighk 3:717de74f6ebd 202 case INCONCLUSIVE:
twighk 3:717de74f6ebd 203 pc.printf("INCONCLUSIVE\n\r");
twighk 3:717de74f6ebd 204 break;
twighk 3:717de74f6ebd 205 default:
twighk 3:717de74f6ebd 206 pc.printf("BUG\n\r");
twighk 3:717de74f6ebd 207 }
twighk 2:45da48fab346 208 }
twighk 0:200635fa1b08 209
twighk 3:717de74f6ebd 210 }
twighk 3:717de74f6ebd 211
twighk 3:717de74f6ebd 212
twighk 3:717de74f6ebd 213 void ledphototransistortest(){
madcowswe 7:4340355261f9 214 DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
madcowswe 7:4340355261f9 215 AnalogIn pt(P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 216 Serial pc(USBTX, USBRX);
twighk 3:717de74f6ebd 217
twighk 3:717de74f6ebd 218 while(true){
twighk 11:bbddc908c78c 219 blue = 0; red = 0;
twighk 11:bbddc908c78c 220 for(int i = 0; i != 5; i++){
twighk 11:bbddc908c78c 221 wait(0.1);
twighk 11:bbddc908c78c 222 pc.printf("Phototransistor Analog is (none): %f \n\r", pt.read());
twighk 11:bbddc908c78c 223 }
twighk 11:bbddc908c78c 224
madcowswe 7:4340355261f9 225 blue = 1; red = 0;
twighk 3:717de74f6ebd 226 for(int i = 0; i != 5; i++){
twighk 3:717de74f6ebd 227 wait(0.1);
twighk 3:717de74f6ebd 228 pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
twighk 3:717de74f6ebd 229 }
madcowswe 7:4340355261f9 230 blue = 0; red = 1;
twighk 3:717de74f6ebd 231 for(int i = 0; i != 5; i++){
twighk 3:717de74f6ebd 232 wait(0.1);
twighk 3:717de74f6ebd 233 pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
twighk 3:717de74f6ebd 234 }
twighk 11:bbddc908c78c 235 blue = 1; red = 1;
twighk 11:bbddc908c78c 236 for(int i = 0; i != 5; i++){
twighk 11:bbddc908c78c 237 wait(0.1);
twighk 11:bbddc908c78c 238 pc.printf("Phototransistor Analog is (both): %f \n\r", pt.read());
twighk 11:bbddc908c78c 239 }
twighk 3:717de74f6ebd 240 }
twighk 3:717de74f6ebd 241 }
twighk 3:717de74f6ebd 242
twighk 3:717de74f6ebd 243 void phototransistortest(){
madcowswe 7:4340355261f9 244 AnalogIn pt(P_COLOR_SENSOR_IN);
twighk 3:717de74f6ebd 245 while(true){
twighk 3:717de74f6ebd 246 wait(0.1);
twighk 3:717de74f6ebd 247 pc.printf("Phototransistor Analog is: %f \n\r", pt.read());
twighk 3:717de74f6ebd 248 }
twighk 3:717de74f6ebd 249
twighk 3:717de74f6ebd 250 }
twighk 3:717de74f6ebd 251
twighk 3:717de74f6ebd 252 void ledtest(){
madcowswe 7:4340355261f9 253 DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
twighk 3:717de74f6ebd 254 while(true){
madcowswe 7:4340355261f9 255 blue = 1; red = 0;
twighk 3:717de74f6ebd 256 wait(0.2);
madcowswe 7:4340355261f9 257 blue = 0; red = 1;
twighk 3:717de74f6ebd 258 wait(0.2);
twighk 3:717de74f6ebd 259
twighk 3:717de74f6ebd 260 }
twighk 3:717de74f6ebd 261 }
twighk 3:717de74f6ebd 262
twighk 1:8119211eae14 263 void armtest(){
twighk 3:717de74f6ebd 264 Arm white(p26), black(p25, false, 0.0005, 180);
twighk 3:717de74f6ebd 265 while(true){
twighk 1:8119211eae14 266 white(0);
twighk 1:8119211eae14 267 black(0);
twighk 1:8119211eae14 268 wait(1);
twighk 1:8119211eae14 269 white(1);
twighk 1:8119211eae14 270 black(1);
twighk 1:8119211eae14 271 wait(1);
twighk 1:8119211eae14 272 }
twighk 1:8119211eae14 273 }
twighk 1:8119211eae14 274
twighk 1:8119211eae14 275
twighk 0:200635fa1b08 276 void motorsandservostest(){
twighk 0:200635fa1b08 277 Encoder Eleft(p27, p28), Eright(p30, p29);
twighk 0:200635fa1b08 278 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 1:8119211eae14 279 Arm sTop(p25), sBottom(p26);
twighk 4:1be0f6c6ceae 280 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 281 const float speed = 0.0;
twighk 0:200635fa1b08 282 const float dspeed = 0.0;
twighk 0:200635fa1b08 283
twighk 0:200635fa1b08 284 Timer servoTimer;
twighk 0:200635fa1b08 285 mleft(speed); mright(speed);
twighk 0:200635fa1b08 286 servoTimer.start();
twighk 0:200635fa1b08 287 while (true){
twighk 0:200635fa1b08 288 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 289 if (Eleft.getPoint() < Eright.getPoint()){
twighk 0:200635fa1b08 290 mleft(speed);
twighk 0:200635fa1b08 291 mright(speed - dspeed);
twighk 0:200635fa1b08 292 } else {
twighk 0:200635fa1b08 293 mright(speed);
twighk 0:200635fa1b08 294 mleft(speed - dspeed);
twighk 0:200635fa1b08 295 }
twighk 0:200635fa1b08 296 if (servoTimer.read() < 1){
twighk 0:200635fa1b08 297 sTop.clockwise();
twighk 0:200635fa1b08 298 } else if (servoTimer.read() < 4) {
twighk 2:45da48fab346 299 sTop.halt();
twighk 0:200635fa1b08 300 } else if (servoTimer.read() < 5) {
twighk 0:200635fa1b08 301 sBottom.anticlockwise();
twighk 0:200635fa1b08 302 //Led=1;
twighk 0:200635fa1b08 303 } else if (servoTimer.read() < 6) {
twighk 0:200635fa1b08 304 sBottom.clockwise();
twighk 0:200635fa1b08 305 //Led=0;
twighk 0:200635fa1b08 306 } else if (servoTimer.read() < 7) {
twighk 2:45da48fab346 307 sBottom.halt();
twighk 0:200635fa1b08 308 }else {
twighk 0:200635fa1b08 309 sTop.anticlockwise();
twighk 0:200635fa1b08 310 }
twighk 0:200635fa1b08 311 if (servoTimer.read() >= 9) servoTimer.reset();
twighk 0:200635fa1b08 312 }
twighk 0:200635fa1b08 313 }
twighk 0:200635fa1b08 314
twighk 2:45da48fab346 315 void motortestline(){
twighk 2:45da48fab346 316 MainMotor mleft(p24,p23), mright(p21,p22);
twighk 2:45da48fab346 317 const float speed = 0.2;
twighk 2:45da48fab346 318 mleft(speed); mright(speed);
twighk 2:45da48fab346 319 while(true) wait(1);
twighk 2:45da48fab346 320 }
twighk 2:45da48fab346 321
twighk 0:200635fa1b08 322 void motorencodetestline(){
madcowswe 13:76c9915db820 323 Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 13:76c9915db820 324 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
twighk 4:1be0f6c6ceae 325 //Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 326 const float speed = 0.2;
twighk 0:200635fa1b08 327 const float dspeed = 0.1;
twighk 0:200635fa1b08 328
twighk 0:200635fa1b08 329 mleft(speed); mright(speed);
twighk 0:200635fa1b08 330 while (true){
twighk 0:200635fa1b08 331 //left 27 cm = 113 -> 0.239 cm/pulse
twighk 0:200635fa1b08 332 //right 27 cm = 72 -> 0.375 cm/pulse
twighk 0:200635fa1b08 333 pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
twighk 0:200635fa1b08 334 if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
twighk 0:200635fa1b08 335 mright(speed - dspeed);
twighk 0:200635fa1b08 336 } else {
twighk 0:200635fa1b08 337 mright(speed + dspeed);
twighk 0:200635fa1b08 338 }
twighk 0:200635fa1b08 339 }
twighk 0:200635fa1b08 340
twighk 0:200635fa1b08 341 }
twighk 0:200635fa1b08 342
twighk 0:200635fa1b08 343 void motorencodetest(){
madcowswe 7:4340355261f9 344 Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 7:4340355261f9 345 MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
twighk 0:200635fa1b08 346 Serial pc(USBTX, USBRX);
twighk 0:200635fa1b08 347
twighk 0:200635fa1b08 348 const float speed = -0.3;
twighk 0:200635fa1b08 349 const int enc = -38;
twighk 0:200635fa1b08 350 while(true){
twighk 0:200635fa1b08 351 mleft(speed); mright(0);
twighk 0:200635fa1b08 352 while(Eleft.getPoint()>enc){
twighk 0:200635fa1b08 353 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 354 }
twighk 0:200635fa1b08 355 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 356 mleft(0); mright(speed);
twighk 0:200635fa1b08 357 while(Eright.getPoint()>enc){
twighk 0:200635fa1b08 358 pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
twighk 0:200635fa1b08 359 }
twighk 0:200635fa1b08 360 Eleft.reset(); Eright.reset();
twighk 0:200635fa1b08 361 }
twighk 0:200635fa1b08 362 }
twighk 0:200635fa1b08 363
twighk 0:200635fa1b08 364 void encodertest(){
madcowswe 15:9c5aaeda36dc 365 Encoder E1(P_ENC_LEFT_A, P_ENC_LEFT_B);
madcowswe 15:9c5aaeda36dc 366 //Encoder E2(P_ENC_RIGHT_A, P_ENC_RIGHT_B);
twighk 0:200635fa1b08 367 Serial pc(USBTX, USBRX);
twighk 3:717de74f6ebd 368 while(true){
twighk 0:200635fa1b08 369 wait(0.1);
madcowswe 15:9c5aaeda36dc 370 pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), 0);//E2.getPoint());
twighk 0:200635fa1b08 371 }
twighk 0:200635fa1b08 372
twighk 0:200635fa1b08 373 }
twighk 0:200635fa1b08 374 void motortest(){
twighk 0:200635fa1b08 375 MainMotor mright(p22,p21), mleft(p23,p24);
twighk 3:717de74f6ebd 376 while(true) {
twighk 0:200635fa1b08 377 wait(1);
twighk 0:200635fa1b08 378 mleft(0.8); mright(0.8);
twighk 0:200635fa1b08 379 wait(1);
twighk 0:200635fa1b08 380 mleft(-0.2); mright(0.2);
twighk 0:200635fa1b08 381 wait(1);
twighk 0:200635fa1b08 382 mleft(0); mright(0);
twighk 0:200635fa1b08 383 }
twighk 0:200635fa1b08 384 }