Asif Sattar / Mbed 2 deprecated mbed_mainboard_source_n

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   USBDevice mbed motor

Fork of mbed_mainboard_source by Asif Sattar

main.cpp@2:2a47186e72c3, 2016-11-11 (annotated)

Committer:
Asif_
Date:
Fri Nov 11 22:13:25 2016 +0000
Revision:
2:2a47186e72c3
Parent:
0:da5127da2ba0 
mem

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kolibakter 0:da5127da2ba0 1 #include "mbed.h"
kolibakter 0:da5127da2ba0 2 #include "pins.h"
kolibakter 0:da5127da2ba0 3 #include "motor.h"
kolibakter 0:da5127da2ba0 4 #include "definitions.h"
kolibakter 0:da5127da2ba0 5 #include "USBSerial.h"
kolibakter 0:da5127da2ba0 6
kolibakter 0:da5127da2ba0 7 typedef void (*VoidArray) ();
kolibakter 0:da5127da2ba0 8
Asif_ 2:2a47186e72c3 9 DigitalOut led(P2_8);
Asif_ 2:2a47186e72c3 10 DigitalOut led2g(P1_18);
Asif_ 2:2a47186e72c3 11 //DigitalOut l(P1_18);
Asif_ 2:2a47186e72c3 12 PwmOut dribbler(P2_4); //Dribbler pwm pin - B1
Asif_ 2:2a47186e72c3 13 DigitalOut charge(P0_10); //Kicker charge pin - A3
Asif_ 2:2a47186e72c3 14 DigitalOut kick(P0_11); //Kicker kick pin - A2
Asif_ 2:2a47186e72c3 15 DigitalIn done(P1_29); // Kicker done pin - A4
Asif_ 2:2a47186e72c3 16 DigitalIn infrared(P3_25); //PWM - GPIO2
kolibakter 0:da5127da2ba0 17 USBSerial pc;
kolibakter 0:da5127da2ba0 18
kolibakter 0:da5127da2ba0 19 Ticker motorPidTicker[NUMBER_OF_MOTORS];
kolibakter 0:da5127da2ba0 20
kolibakter 0:da5127da2ba0 21 char buf[16];
kolibakter 0:da5127da2ba0 22 bool serialData = false;
kolibakter 0:da5127da2ba0 23 int serialCount = 0;
kolibakter 0:da5127da2ba0 24
kolibakter 0:da5127da2ba0 25 volatile int16_t motorTicks[NUMBER_OF_MOTORS];
kolibakter 0:da5127da2ba0 26 volatile uint8_t motorEncNow[NUMBER_OF_MOTORS];
kolibakter 0:da5127da2ba0 27 volatile uint8_t motorEncLast[NUMBER_OF_MOTORS];
kolibakter 0:da5127da2ba0 28
kolibakter 0:da5127da2ba0 29 Motor motors[NUMBER_OF_MOTORS];
kolibakter 0:da5127da2ba0 30
kolibakter 0:da5127da2ba0 31 void serialInterrupt();
kolibakter 0:da5127da2ba0 32 void parseCommad(char *command);
kolibakter 0:da5127da2ba0 33
kolibakter 0:da5127da2ba0 34 void motor0EncTick();
kolibakter 0:da5127da2ba0 35 void motor1EncTick();
kolibakter 0:da5127da2ba0 36 void motor2EncTick();
kolibakter 0:da5127da2ba0 37 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 38 void motor3EncTick();
kolibakter 0:da5127da2ba0 39 #endif
kolibakter 0:da5127da2ba0 40
kolibakter 0:da5127da2ba0 41 void motor0PidTick();
kolibakter 0:da5127da2ba0 42 void motor1PidTick();
kolibakter 0:da5127da2ba0 43 void motor2PidTick();
kolibakter 0:da5127da2ba0 44 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 45 void motor3PidTick();
kolibakter 0:da5127da2ba0 46 #endif
kolibakter 0:da5127da2ba0 47
kolibakter 0:da5127da2ba0 48 int main() {
Asif_ 2:2a47186e72c3 49
kolibakter 0:da5127da2ba0 50 void (*encTicker[])() = {
kolibakter 0:da5127da2ba0 51 motor0EncTick,
kolibakter 0:da5127da2ba0 52 motor1EncTick,
kolibakter 0:da5127da2ba0 53 motor2EncTick,
kolibakter 0:da5127da2ba0 54 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 55 motor3EncTick
kolibakter 0:da5127da2ba0 56 #endif
kolibakter 0:da5127da2ba0 57 };
kolibakter 0:da5127da2ba0 58
kolibakter 0:da5127da2ba0 59 VoidArray pidTicker[] = {
kolibakter 0:da5127da2ba0 60 motor0PidTick,
kolibakter 0:da5127da2ba0 61 motor1PidTick,
kolibakter 0:da5127da2ba0 62 motor2PidTick,
kolibakter 0:da5127da2ba0 63 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 64 motor3PidTick
kolibakter 0:da5127da2ba0 65 #endif
kolibakter 0:da5127da2ba0 66 };
kolibakter 0:da5127da2ba0 67
kolibakter 0:da5127da2ba0 68 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
kolibakter 0:da5127da2ba0 69 MotorEncA[i]->mode(PullNone);
kolibakter 0:da5127da2ba0 70 MotorEncB[i]->mode(PullNone);
kolibakter 0:da5127da2ba0 71
kolibakter 0:da5127da2ba0 72 motors[i] = Motor(&pc, MotorPwm[i], MotorDir1[i], MotorDir2[i], MotorFault[i]);
kolibakter 0:da5127da2ba0 73
kolibakter 0:da5127da2ba0 74 motorTicks[i] = 0;
kolibakter 0:da5127da2ba0 75 motorEncNow[i] = 0;
kolibakter 0:da5127da2ba0 76 motorEncLast[i] = 0;
kolibakter 0:da5127da2ba0 77
kolibakter 0:da5127da2ba0 78 MotorEncA[i]->rise(encTicker[i]);
kolibakter 0:da5127da2ba0 79 MotorEncA[i]->fall(encTicker[i]);
kolibakter 0:da5127da2ba0 80 MotorEncB[i]->rise(encTicker[i]);
kolibakter 0:da5127da2ba0 81 MotorEncB[i]->fall(encTicker[i]);
kolibakter 0:da5127da2ba0 82
kolibakter 0:da5127da2ba0 83 motorPidTicker[i].attach(pidTicker[i], 0.1);
kolibakter 0:da5127da2ba0 84
kolibakter 0:da5127da2ba0 85 motors[i].init();
kolibakter 0:da5127da2ba0 86 }
kolibakter 0:da5127da2ba0 87
Asif_ 2:2a47186e72c3 88 //pc.printf("Start\n");
Asif_ 2:2a47186e72c3 89 /*motors[0].setSpeed(100);
Asif_ 2:2a47186e72c3 90 motors[1].setSpeed(50);
Asif_ 2:2a47186e72c3 91 motors[2].setSpeed(150);*/
Asif_ 2:2a47186e72c3 92
Asif_ 2:2a47186e72c3 93 /*while (pc.readable())
Asif_ 2:2a47186e72c3 94 {
Asif_ 2:2a47186e72c3 95 pc.printf("readdddd");
Asif_ 2:2a47186e72c3 96 }*/
kolibakter 0:da5127da2ba0 97
kolibakter 0:da5127da2ba0 98 pc.attach(&serialInterrupt);
kolibakter 0:da5127da2ba0 99 int count = 0;
Asif_ 2:2a47186e72c3 100
Asif_ 2:2a47186e72c3 101
Asif_ 2:2a47186e72c3 102 /*dribbler.pulsewidth_us(100);
Asif_ 2:2a47186e72c3 103 wait(3.0);
Asif_ 2:2a47186e72c3 104 dribbler.pulsewidth_us(140);
Asif_ 2:2a47186e72c3 105 wait(5.0);
Asif_ 2:2a47186e72c3 106 charge = 1;
Asif_ 2:2a47186e72c3 107 wait(8.0);
Asif_ 2:2a47186e72c3 108 charge = 0;
Asif_ 2:2a47186e72c3 109 wait(1);
Asif_ 2:2a47186e72c3 110 kick = 1;
Asif_ 2:2a47186e72c3 111 wait_ms(200);
Asif_ 2:2a47186e72c3 112 kick = 0;*/
Asif_ 2:2a47186e72c3 113
Asif_ 2:2a47186e72c3 114 //pc.printf('true');
kolibakter 0:da5127da2ba0 115 while(1) {
Asif_ 2:2a47186e72c3 116 /*if (count % 20 == 0) {
kolibakter 0:da5127da2ba0 117 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
kolibakter 0:da5127da2ba0 118 pc.printf("s%d:%d\n", i, motors[i].getSpeed());
kolibakter 0:da5127da2ba0 119 }
Asif_ 2:2a47186e72c3 120 }*/
kolibakter 0:da5127da2ba0 121 if (serialData) {
kolibakter 0:da5127da2ba0 122 char temp[16];
kolibakter 0:da5127da2ba0 123 memcpy(temp, buf, 16);
kolibakter 0:da5127da2ba0 124 memset(buf, 0, 16);
kolibakter 0:da5127da2ba0 125 serialData = false;
kolibakter 0:da5127da2ba0 126 parseCommad(temp);
kolibakter 0:da5127da2ba0 127 }
Asif_ 2:2a47186e72c3 128 if (infrared)
Asif_ 2:2a47186e72c3 129 {
Asif_ 2:2a47186e72c3 130 led2g = 0;
Asif_ 2:2a47186e72c3 131 }
Asif_ 2:2a47186e72c3 132 else {led2g = 1;}
Asif_ 2:2a47186e72c3 133 //else {pc.printf("false");}
kolibakter 0:da5127da2ba0 134 //motors[0].pid(motor0Ticks);
kolibakter 0:da5127da2ba0 135 //motor0Ticks = 0;
kolibakter 0:da5127da2ba0 136 wait_ms(50);
kolibakter 0:da5127da2ba0 137 count++;
Asif_ 2:2a47186e72c3 138 /*while (pc.writeable())
Asif_ 2:2a47186e72c3 139 {
Asif_ 2:2a47186e72c3 140 pc.printf("test_write \n");
Asif_ 2:2a47186e72c3 141 }
Asif_ 2:2a47186e72c3 142
Asif_ 2:2a47186e72c3 143 while (pc.readable())
Asif_ 2:2a47186e72c3 144 {
Asif_ 2:2a47186e72c3 145 pc.printf("test_read \n");
Asif_ 2:2a47186e72c3 146 }*/
kolibakter 0:da5127da2ba0 147
kolibakter 0:da5127da2ba0 148 //pc.printf("buf: %s\n", buf);
kolibakter 0:da5127da2ba0 149 //pc.printf("Loop\n");
kolibakter 0:da5127da2ba0 150 }
kolibakter 0:da5127da2ba0 151 }
Asif_ 2:2a47186e72c3 152 //uint buf[128];
kolibakter 0:da5127da2ba0 153 void serialInterrupt(){
kolibakter 0:da5127da2ba0 154 while(pc.readable()) {
kolibakter 0:da5127da2ba0 155 buf[serialCount] = pc.getc();
kolibakter 0:da5127da2ba0 156 serialCount++;
kolibakter 0:da5127da2ba0 157 }
kolibakter 0:da5127da2ba0 158 if (buf[serialCount - 1] == '\n') {
kolibakter 0:da5127da2ba0 159 serialData = true;
kolibakter 0:da5127da2ba0 160 serialCount = 0;
kolibakter 0:da5127da2ba0 161 }
kolibakter 0:da5127da2ba0 162 }
kolibakter 0:da5127da2ba0 163
kolibakter 0:da5127da2ba0 164 void parseCommad (char *command) {
Asif_ 2:2a47186e72c3 165 char *searcha = command;
Asif_ 2:2a47186e72c3 166 char *a;
Asif_ 2:2a47186e72c3 167 int indexa;
Asif_ 2:2a47186e72c3 168 a=strchr(searcha, 'a');
Asif_ 2:2a47186e72c3 169 indexa = int(a - searcha);
Asif_ 2:2a47186e72c3 170
Asif_ 2:2a47186e72c3 171 char *searchb = command;
Asif_ 2:2a47186e72c3 172 char *b;
Asif_ 2:2a47186e72c3 173 int indexb;
Asif_ 2:2a47186e72c3 174 b=strchr(searchb, 'b');
Asif_ 2:2a47186e72c3 175 indexb = int(b - searchb);
Asif_ 2:2a47186e72c3 176
Asif_ 2:2a47186e72c3 177 //dribbler.period(0.058f);
Asif_ 2:2a47186e72c3 178
Asif_ 2:2a47186e72c3 179 char *searchc = command;
Asif_ 2:2a47186e72c3 180 char *c;
Asif_ 2:2a47186e72c3 181 int indexc;
Asif_ 2:2a47186e72c3 182 c=strchr(searchc, 'c');
Asif_ 2:2a47186e72c3 183 indexc = int(c - searchc);
Asif_ 2:2a47186e72c3 184
Asif_ 2:2a47186e72c3 185 /*char *searchd = command;
Asif_ 2:2a47186e72c3 186 char *d;
Asif_ 2:2a47186e72c3 187 int indexd;
Asif_ 2:2a47186e72c3 188 d=strchr(searchd, 'd');
Asif_ 2:2a47186e72c3 189 indexd = int(d - searchd);*/
Asif_ 2:2a47186e72c3 190
Asif_ 2:2a47186e72c3 191 int16_t speeda = atoi(command + (indexa+1));
Asif_ 2:2a47186e72c3 192 motors[0].pid_on = 1;
Asif_ 2:2a47186e72c3 193 motors[0].setSpeed(speeda);
Asif_ 2:2a47186e72c3 194
Asif_ 2:2a47186e72c3 195 int16_t speedb = atoi(command + (indexb+1));
Asif_ 2:2a47186e72c3 196 motors[1].pid_on = 1;
Asif_ 2:2a47186e72c3 197 motors[1].setSpeed(speedb);
Asif_ 2:2a47186e72c3 198
Asif_ 2:2a47186e72c3 199 int16_t speedc = atoi(command + (indexc+1));
Asif_ 2:2a47186e72c3 200 motors[2].pid_on = 1;
Asif_ 2:2a47186e72c3 201 motors[2].setSpeed(speedc);
Asif_ 2:2a47186e72c3 202
Asif_ 2:2a47186e72c3 203 /*int16_t speedd = atoi(command + (indexd+1));
Asif_ 2:2a47186e72c3 204 if (speedd == 0)
Asif_ 2:2a47186e72c3 205 {
Asif_ 2:2a47186e72c3 206 dribbler.pulsewidth_us(100);
Asif_ 2:2a47186e72c3 207 wait(3.0);
Asif_ 2:2a47186e72c3 208 }
Asif_ 2:2a47186e72c3 209 if (speedd == 1)
Asif_ 2:2a47186e72c3 210 {
Asif_ 2:2a47186e72c3 211 dribbler.pulsewidth_us(135);
Asif_ 2:2a47186e72c3 212 }*/
Asif_ 2:2a47186e72c3 213
Asif_ 2:2a47186e72c3 214
Asif_ 2:2a47186e72c3 215 /*if (command[0] == 's' && command[1] == 'd' && command[2] == '0') {
Asif_ 2:2a47186e72c3 216 int16_t speed = atoi(command + 3);
kolibakter 0:da5127da2ba0 217 motors[0].pid_on = 1;
kolibakter 0:da5127da2ba0 218 motors[0].setSpeed(speed);
kolibakter 0:da5127da2ba0 219 }
Asif_ 2:2a47186e72c3 220 if (command[0] == 's' && command[1] == 'd' && command[2] == '1') {
Asif_ 2:2a47186e72c3 221 int16_t speed = atoi(command + 3);
Asif_ 2:2a47186e72c3 222 motors[1].pid_on = 1;
Asif_ 2:2a47186e72c3 223 motors[1].setSpeed(speed);
Asif_ 2:2a47186e72c3 224 }
Asif_ 2:2a47186e72c3 225 if (command[0] == 's' && command[1] == 'd' && command[2] == '2') {
Asif_ 2:2a47186e72c3 226 int16_t speed = atoi(command + 3);
Asif_ 2:2a47186e72c3 227 motors[2].pid_on = 1;
Asif_ 2:2a47186e72c3 228 motors[2].setSpeed(speed);
Asif_ 2:2a47186e72c3 229 }
Asif_ 2:2a47186e72c3 230 if (command[0] == 'f') {
Asif_ 2:2a47186e72c3 231 int16_t speed = atoi(command + 1);
Asif_ 2:2a47186e72c3 232 motors[0].pid_on = 1;
Asif_ 2:2a47186e72c3 233 motors[0].setSpeed(-speed);
Asif_ 2:2a47186e72c3 234 motors[2].pid_on = 1;
Asif_ 2:2a47186e72c3 235 motors[2].setSpeed(speed);
Asif_ 2:2a47186e72c3 236 }
Asif_ 2:2a47186e72c3 237 if (command[0] == 'b' && command[1] == '1' && command[2] == '2') {
Asif_ 2:2a47186e72c3 238 int16_t speed = atoi(command + 3);
Asif_ 2:2a47186e72c3 239 motors[1].pid_on = 1;
Asif_ 2:2a47186e72c3 240 motors[1].setSpeed(speed);
Asif_ 2:2a47186e72c3 241 motors[2].pid_on = 1;
Asif_ 2:2a47186e72c3 242 motors[2].setSpeed(speed);
Asif_ 2:2a47186e72c3 243 }
Asif_ 2:2a47186e72c3 244
Asif_ 2:2a47186e72c3 245 if (command[0] == 'b' && command[1] == '0' && command[2] == '1') {
Asif_ 2:2a47186e72c3 246 int16_t speed = atoi(command + 3);
Asif_ 2:2a47186e72c3 247 motors[0].pid_on = 1;
Asif_ 2:2a47186e72c3 248 motors[0].setSpeed(speed);
Asif_ 2:2a47186e72c3 249 motors[1].pid_on = 1;
Asif_ 2:2a47186e72c3 250 motors[1].setSpeed(speed);
Asif_ 2:2a47186e72c3 251 }
Asif_ 2:2a47186e72c3 252 if (command[0] == 'b' && command[1] == '0' && command[2] == '2') {
Asif_ 2:2a47186e72c3 253 int16_t speed = atoi(command + 3);
Asif_ 2:2a47186e72c3 254 motors[0].pid_on = 1;
Asif_ 2:2a47186e72c3 255 motors[0].setSpeed(speed);
Asif_ 2:2a47186e72c3 256 motors[2].pid_on = 1;
Asif_ 2:2a47186e72c3 257 motors[2].setSpeed(speed);
Asif_ 2:2a47186e72c3 258 }
Asif_ 2:2a47186e72c3 259 if (command[0] == 'a') {
Asif_ 2:2a47186e72c3 260 int16_t speed = atoi(command + 1);
Asif_ 2:2a47186e72c3 261 motors[0].pid_on = 1;
Asif_ 2:2a47186e72c3 262 motors[0].setSpeed(speed);
Asif_ 2:2a47186e72c3 263 motors[1].pid_on = 1;
Asif_ 2:2a47186e72c3 264 motors[1].setSpeed(speed);
Asif_ 2:2a47186e72c3 265 motors[2].pid_on = 1;
Asif_ 2:2a47186e72c3 266 motors[2].setSpeed(speed);
Asif_ 2:2a47186e72c3 267 } */
Asif_ 2:2a47186e72c3 268
Asif_ 2:2a47186e72c3 269
kolibakter 0:da5127da2ba0 270 if (command[0] == 's') {
kolibakter 0:da5127da2ba0 271 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
kolibakter 0:da5127da2ba0 272 pc.printf("s%d:%d\n", i, motors[i].getSpeed());
kolibakter 0:da5127da2ba0 273 }
kolibakter 0:da5127da2ba0 274 } else if (command[0] == 'w' && command[1] == 'l') {
kolibakter 0:da5127da2ba0 275 int16_t speed = atoi(command + 2);
kolibakter 0:da5127da2ba0 276 motors[0].pid_on = 0;
kolibakter 0:da5127da2ba0 277 if (speed < 0) motors[0].backward(-1*speed/255.0);
kolibakter 0:da5127da2ba0 278 else motors[0].forward(speed/255.0);
kolibakter 0:da5127da2ba0 279 } else if (command[0] == 'p' && command[1] == 'p') {
kolibakter 0:da5127da2ba0 280 uint8_t pGain = atoi(command + 2);
kolibakter 0:da5127da2ba0 281 motors[0].pgain = pGain;
kolibakter 0:da5127da2ba0 282 } else if (command[0] == 'p' && command[1] == 'i') {
kolibakter 0:da5127da2ba0 283 uint8_t iGain = atoi(command + 2);
kolibakter 0:da5127da2ba0 284 motors[0].igain = iGain;
kolibakter 0:da5127da2ba0 285 } else if (command[0] == 'p' && command[1] == 'd') {
kolibakter 0:da5127da2ba0 286 uint8_t dGain = atoi(command + 2);
kolibakter 0:da5127da2ba0 287 motors[0].dgain = dGain;
kolibakter 0:da5127da2ba0 288 } else if (command[0] == 'p') {
kolibakter 0:da5127da2ba0 289 char gain[20];
kolibakter 0:da5127da2ba0 290 motors[0].getPIDGain(gain);
kolibakter 0:da5127da2ba0 291 pc.printf("%s\n", gain);
kolibakter 0:da5127da2ba0 292 }
kolibakter 0:da5127da2ba0 293 }
kolibakter 0:da5127da2ba0 294
kolibakter 0:da5127da2ba0 295 MOTOR_ENC_TICK(0)
kolibakter 0:da5127da2ba0 296 MOTOR_ENC_TICK(1)
kolibakter 0:da5127da2ba0 297 MOTOR_ENC_TICK(2)
kolibakter 0:da5127da2ba0 298 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 299 MOTOR_ENC_TICK(3)
kolibakter 0:da5127da2ba0 300 #endif
kolibakter 0:da5127da2ba0 301
kolibakter 0:da5127da2ba0 302 MOTOR_PID_TICK(0)
kolibakter 0:da5127da2ba0 303 MOTOR_PID_TICK(1)
kolibakter 0:da5127da2ba0 304 MOTOR_PID_TICK(2)
kolibakter 0:da5127da2ba0 305 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 306 MOTOR_PID_TICK(3)
kolibakter 0:da5127da2ba0 307 #endif