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