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