Elmar Abbasov / Mbed 2 deprecated Luna

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@3:ff1267cf6b03, 2016-11-28 (annotated)

Committer:
ubicray
Date:
Mon Nov 28 17:19:57 2016 +0000
Revision:
3:ff1267cf6b03
Parent:
2:2a47186e72c3 
version1

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);
ubicray 3:ff1267cf6b03 12 PwmOut dribbler(P2_5); //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;
ubicray 3:ff1267cf6b03 24 int m=1;
kolibakter 0:da5127da2ba0 25
kolibakter 0:da5127da2ba0 26 volatile int16_t motorTicks[NUMBER_OF_MOTORS];
kolibakter 0:da5127da2ba0 27 volatile uint8_t motorEncNow[NUMBER_OF_MOTORS];
kolibakter 0:da5127da2ba0 28 volatile uint8_t motorEncLast[NUMBER_OF_MOTORS];
kolibakter 0:da5127da2ba0 29
kolibakter 0:da5127da2ba0 30 Motor motors[NUMBER_OF_MOTORS];
kolibakter 0:da5127da2ba0 31
kolibakter 0:da5127da2ba0 32 void serialInterrupt();
kolibakter 0:da5127da2ba0 33 void parseCommad(char *command);
kolibakter 0:da5127da2ba0 34
kolibakter 0:da5127da2ba0 35 void motor0EncTick();
kolibakter 0:da5127da2ba0 36 void motor1EncTick();
kolibakter 0:da5127da2ba0 37 void motor2EncTick();
kolibakter 0:da5127da2ba0 38 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 39 void motor3EncTick();
kolibakter 0:da5127da2ba0 40 #endif
kolibakter 0:da5127da2ba0 41
kolibakter 0:da5127da2ba0 42 void motor0PidTick();
kolibakter 0:da5127da2ba0 43 void motor1PidTick();
kolibakter 0:da5127da2ba0 44 void motor2PidTick();
kolibakter 0:da5127da2ba0 45 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 46 void motor3PidTick();
kolibakter 0:da5127da2ba0 47 #endif
kolibakter 0:da5127da2ba0 48
kolibakter 0:da5127da2ba0 49 int main() {
Asif_ 2:2a47186e72c3 50
kolibakter 0:da5127da2ba0 51 void (*encTicker[])() = {
kolibakter 0:da5127da2ba0 52 motor0EncTick,
kolibakter 0:da5127da2ba0 53 motor1EncTick,
kolibakter 0:da5127da2ba0 54 motor2EncTick,
kolibakter 0:da5127da2ba0 55 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 56 motor3EncTick
kolibakter 0:da5127da2ba0 57 #endif
kolibakter 0:da5127da2ba0 58 };
kolibakter 0:da5127da2ba0 59
kolibakter 0:da5127da2ba0 60 VoidArray pidTicker[] = {
kolibakter 0:da5127da2ba0 61 motor0PidTick,
kolibakter 0:da5127da2ba0 62 motor1PidTick,
kolibakter 0:da5127da2ba0 63 motor2PidTick,
kolibakter 0:da5127da2ba0 64 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 65 motor3PidTick
kolibakter 0:da5127da2ba0 66 #endif
kolibakter 0:da5127da2ba0 67 };
kolibakter 0:da5127da2ba0 68
kolibakter 0:da5127da2ba0 69 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
kolibakter 0:da5127da2ba0 70 MotorEncA[i]->mode(PullNone);
kolibakter 0:da5127da2ba0 71 MotorEncB[i]->mode(PullNone);
kolibakter 0:da5127da2ba0 72
kolibakter 0:da5127da2ba0 73 motors[i] = Motor(&pc, MotorPwm[i], MotorDir1[i], MotorDir2[i], MotorFault[i]);
kolibakter 0:da5127da2ba0 74
kolibakter 0:da5127da2ba0 75 motorTicks[i] = 0;
kolibakter 0:da5127da2ba0 76 motorEncNow[i] = 0;
kolibakter 0:da5127da2ba0 77 motorEncLast[i] = 0;
kolibakter 0:da5127da2ba0 78
kolibakter 0:da5127da2ba0 79 MotorEncA[i]->rise(encTicker[i]);
kolibakter 0:da5127da2ba0 80 MotorEncA[i]->fall(encTicker[i]);
kolibakter 0:da5127da2ba0 81 MotorEncB[i]->rise(encTicker[i]);
kolibakter 0:da5127da2ba0 82 MotorEncB[i]->fall(encTicker[i]);
kolibakter 0:da5127da2ba0 83
kolibakter 0:da5127da2ba0 84 motorPidTicker[i].attach(pidTicker[i], 0.1);
kolibakter 0:da5127da2ba0 85
kolibakter 0:da5127da2ba0 86 motors[i].init();
kolibakter 0:da5127da2ba0 87 }
kolibakter 0:da5127da2ba0 88
Asif_ 2:2a47186e72c3 89 //pc.printf("Start\n");
Asif_ 2:2a47186e72c3 90 /*motors[0].setSpeed(100);
Asif_ 2:2a47186e72c3 91 motors[1].setSpeed(50);
Asif_ 2:2a47186e72c3 92 motors[2].setSpeed(150);*/
Asif_ 2:2a47186e72c3 93
Asif_ 2:2a47186e72c3 94 /*while (pc.readable())
Asif_ 2:2a47186e72c3 95 {
Asif_ 2:2a47186e72c3 96 pc.printf("readdddd");
Asif_ 2:2a47186e72c3 97 }*/
kolibakter 0:da5127da2ba0 98
kolibakter 0:da5127da2ba0 99 pc.attach(&serialInterrupt);
kolibakter 0:da5127da2ba0 100 int count = 0;
Asif_ 2:2a47186e72c3 101
Asif_ 2:2a47186e72c3 102
ubicray 3:ff1267cf6b03 103 //dribbler.pulsewidth_us(100);
ubicray 3:ff1267cf6b03 104 // wait(3.0);
ubicray 3:ff1267cf6b03 105 // dribbler.pulsewidth_us(140);
ubicray 3:ff1267cf6b03 106 // wait(5.0);
ubicray 3:ff1267cf6b03 107 // charge = 1;
ubicray 3:ff1267cf6b03 108 // wait(8.0);
ubicray 3:ff1267cf6b03 109 // charge = 0;
ubicray 3:ff1267cf6b03 110 // wait(1);
ubicray 3:ff1267cf6b03 111 // kick = 1;
ubicray 3:ff1267cf6b03 112 // wait_ms(200);
ubicray 3:ff1267cf6b03 113 // kick = 0;
Asif_ 2:2a47186e72c3 114
Asif_ 2:2a47186e72c3 115 //pc.printf('true');
ubicray 3:ff1267cf6b03 116
ubicray 3:ff1267cf6b03 117 dribbler.period_ms(2);
kolibakter 0:da5127da2ba0 118 while(1) {
Asif_ 2:2a47186e72c3 119 /*if (count % 20 == 0) {
kolibakter 0:da5127da2ba0 120 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
kolibakter 0:da5127da2ba0 121 pc.printf("s%d:%d\n", i, motors[i].getSpeed());
kolibakter 0:da5127da2ba0 122 }
Asif_ 2:2a47186e72c3 123 }*/
kolibakter 0:da5127da2ba0 124 if (serialData) {
kolibakter 0:da5127da2ba0 125 char temp[16];
kolibakter 0:da5127da2ba0 126 memcpy(temp, buf, 16);
kolibakter 0:da5127da2ba0 127 memset(buf, 0, 16);
kolibakter 0:da5127da2ba0 128 serialData = false;
kolibakter 0:da5127da2ba0 129 parseCommad(temp);
kolibakter 0:da5127da2ba0 130 }
Asif_ 2:2a47186e72c3 131 if (infrared)
Asif_ 2:2a47186e72c3 132 {
Asif_ 2:2a47186e72c3 133 led2g = 0;
Asif_ 2:2a47186e72c3 134 }
Asif_ 2:2a47186e72c3 135 else {led2g = 1;}
Asif_ 2:2a47186e72c3 136 //else {pc.printf("false");}
kolibakter 0:da5127da2ba0 137 //motors[0].pid(motor0Ticks);
kolibakter 0:da5127da2ba0 138 //motor0Ticks = 0;
kolibakter 0:da5127da2ba0 139 wait_ms(50);
kolibakter 0:da5127da2ba0 140 count++;
Asif_ 2:2a47186e72c3 141 /*while (pc.writeable())
Asif_ 2:2a47186e72c3 142 {
Asif_ 2:2a47186e72c3 143 pc.printf("test_write \n");
Asif_ 2:2a47186e72c3 144 }
Asif_ 2:2a47186e72c3 145
Asif_ 2:2a47186e72c3 146 while (pc.readable())
Asif_ 2:2a47186e72c3 147 {
Asif_ 2:2a47186e72c3 148 pc.printf("test_read \n");
Asif_ 2:2a47186e72c3 149 }*/
kolibakter 0:da5127da2ba0 150
kolibakter 0:da5127da2ba0 151 //pc.printf("buf: %s\n", buf);
kolibakter 0:da5127da2ba0 152 //pc.printf("Loop\n");
kolibakter 0:da5127da2ba0 153 }
kolibakter 0:da5127da2ba0 154 }
Asif_ 2:2a47186e72c3 155 //uint buf[128];
kolibakter 0:da5127da2ba0 156 void serialInterrupt(){
kolibakter 0:da5127da2ba0 157 while(pc.readable()) {
kolibakter 0:da5127da2ba0 158 buf[serialCount] = pc.getc();
kolibakter 0:da5127da2ba0 159 serialCount++;
kolibakter 0:da5127da2ba0 160 }
kolibakter 0:da5127da2ba0 161 if (buf[serialCount - 1] == '\n') {
kolibakter 0:da5127da2ba0 162 serialData = true;
kolibakter 0:da5127da2ba0 163 serialCount = 0;
kolibakter 0:da5127da2ba0 164 }
kolibakter 0:da5127da2ba0 165 }
kolibakter 0:da5127da2ba0 166
kolibakter 0:da5127da2ba0 167 void parseCommad (char *command) {
ubicray 3:ff1267cf6b03 168 if (command[0] == 'c' && command[1] == 'a') {
ubicray 3:ff1267cf6b03 169 int16_t speed = atoi(command + 2);
ubicray 3:ff1267cf6b03 170 motors[0].pid_on = 1;
ubicray 3:ff1267cf6b03 171 motors[1].pid_on = 1;
ubicray 3:ff1267cf6b03 172 motors[2].pid_on = 1;
ubicray 3:ff1267cf6b03 173 motors[0].setSpeed(speed);
ubicray 3:ff1267cf6b03 174 motors[1].setSpeed(speed);
ubicray 3:ff1267cf6b03 175 motors[2].setSpeed(speed);
Asif_ 2:2a47186e72c3 176 }
ubicray 3:ff1267cf6b03 177 if (command[0] == 'm' && command[1] == 'f') {
ubicray 3:ff1267cf6b03 178 int16_t speed = atoi(command + 2);
kolibakter 0:da5127da2ba0 179 motors[0].pid_on = 1;
Asif_ 2:2a47186e72c3 180 motors[1].pid_on = 1;
ubicray 3:ff1267cf6b03 181 motors[0].setSpeed(speed*-1);
Asif_ 2:2a47186e72c3 182 motors[1].setSpeed(speed);
Asif_ 2:2a47186e72c3 183 }
ubicray 3:ff1267cf6b03 184 if (command[0] == 't') {
ubicray 3:ff1267cf6b03 185 int16_t speed = atoi(command + 1);
Asif_ 2:2a47186e72c3 186 motors[2].pid_on = 1;
Asif_ 2:2a47186e72c3 187 motors[2].setSpeed(speed);
Asif_ 2:2a47186e72c3 188 }
ubicray 3:ff1267cf6b03 189 // l+002r-112b+502
ubicray 3:ff1267cf6b03 190 else if (command[0] == 'l')
ubicray 3:ff1267cf6b03 191 {
ubicray 3:ff1267cf6b03 192 int16_t lspeed = atoi(command + 1);
Asif_ 2:2a47186e72c3 193 motors[0].pid_on = 1;
ubicray 3:ff1267cf6b03 194 motors[0].setSpeed(lspeed);
ubicray 3:ff1267cf6b03 195 int16_t rspeed = atoi(command + 6);
ubicray 3:ff1267cf6b03 196 motors[1].pid_on = 1;
ubicray 3:ff1267cf6b03 197 motors[1].setSpeed(rspeed);
ubicray 3:ff1267cf6b03 198 int16_t bspeed = atoi(command + 11);
Asif_ 2:2a47186e72c3 199 motors[2].pid_on = 1;
ubicray 3:ff1267cf6b03 200 motors[2].setSpeed(bspeed);
Asif_ 2:2a47186e72c3 201 }
ubicray 3:ff1267cf6b03 202 else if (command[0] == 'c' && command[1] == 's' && command[2] == 'd') {
Asif_ 2:2a47186e72c3 203 int16_t speed = atoi(command + 3);
Asif_ 2:2a47186e72c3 204 motors[0].pid_on = 1;
Asif_ 2:2a47186e72c3 205 motors[1].pid_on = 1;
Asif_ 2:2a47186e72c3 206 motors[2].pid_on = 1;
ubicray 3:ff1267cf6b03 207 motors[0].setSpeed(0);
ubicray 3:ff1267cf6b03 208 motors[1].setSpeed(speed * -1);
Asif_ 2:2a47186e72c3 209 motors[2].setSpeed(speed);
ubicray 3:ff1267cf6b03 210 }
kolibakter 0:da5127da2ba0 211 if (command[0] == 's') {
kolibakter 0:da5127da2ba0 212 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
kolibakter 0:da5127da2ba0 213 pc.printf("s%d:%d\n", i, motors[i].getSpeed());
kolibakter 0:da5127da2ba0 214 }
ubicray 3:ff1267cf6b03 215 }
ubicray 3:ff1267cf6b03 216 else if (command[0] == 'p' && command[1] == 'p') {
kolibakter 0:da5127da2ba0 217 uint8_t pGain = atoi(command + 2);
kolibakter 0:da5127da2ba0 218 motors[0].pgain = pGain;
ubicray 3:ff1267cf6b03 219 motors[1].pgain = pGain;
ubicray 3:ff1267cf6b03 220 motors[2].pgain = pGain;
kolibakter 0:da5127da2ba0 221 } else if (command[0] == 'p' && command[1] == 'i') {
kolibakter 0:da5127da2ba0 222 uint8_t iGain = atoi(command + 2);
kolibakter 0:da5127da2ba0 223 motors[0].igain = iGain;
ubicray 3:ff1267cf6b03 224 motors[1].igain = iGain;
ubicray 3:ff1267cf6b03 225 motors[2].igain = iGain;
kolibakter 0:da5127da2ba0 226 } else if (command[0] == 'p' && command[1] == 'd') {
kolibakter 0:da5127da2ba0 227 uint8_t dGain = atoi(command + 2);
kolibakter 0:da5127da2ba0 228 motors[0].dgain = dGain;
ubicray 3:ff1267cf6b03 229 motors[1].dgain = dGain;
ubicray 3:ff1267cf6b03 230 motors[2].dgain = dGain;
kolibakter 0:da5127da2ba0 231 } else if (command[0] == 'p') {
kolibakter 0:da5127da2ba0 232 char gain[20];
kolibakter 0:da5127da2ba0 233 motors[0].getPIDGain(gain);
kolibakter 0:da5127da2ba0 234 pc.printf("%s\n", gain);
kolibakter 0:da5127da2ba0 235 }
ubicray 3:ff1267cf6b03 236 else if (command[0] == 'd') {
ubicray 3:ff1267cf6b03 237 int PWM = atoi(command+1);
ubicray 3:ff1267cf6b03 238 dribbler.pulsewidth_us(PWM);
ubicray 3:ff1267cf6b03 239 }
ubicray 3:ff1267cf6b03 240 else if (command[0] == 'j') {
ubicray 3:ff1267cf6b03 241 charge.write(1);
ubicray 3:ff1267cf6b03 242 }
ubicray 3:ff1267cf6b03 243 else if (command[0] == 'l') {
ubicray 3:ff1267cf6b03 244 charge.write(0);
ubicray 3:ff1267cf6b03 245 wait_ms(1);
ubicray 3:ff1267cf6b03 246 kick.write(1);
ubicray 3:ff1267cf6b03 247 wait_ms(m);
ubicray 3:ff1267cf6b03 248 kick.write(0);
ubicray 3:ff1267cf6b03 249 }
ubicray 3:ff1267cf6b03 250 else if (command[0] == 'k'){
ubicray 3:ff1267cf6b03 251 kick.write(0);
ubicray 3:ff1267cf6b03 252 }
ubicray 3:ff1267cf6b03 253 else if (command[0] == 'm'){
ubicray 3:ff1267cf6b03 254 m = atoi(command+1);
ubicray 3:ff1267cf6b03 255 }
kolibakter 0:da5127da2ba0 256 }
kolibakter 0:da5127da2ba0 257
kolibakter 0:da5127da2ba0 258 MOTOR_ENC_TICK(0)
kolibakter 0:da5127da2ba0 259 MOTOR_ENC_TICK(1)
kolibakter 0:da5127da2ba0 260 MOTOR_ENC_TICK(2)
kolibakter 0:da5127da2ba0 261 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 262 MOTOR_ENC_TICK(3)
kolibakter 0:da5127da2ba0 263 #endif
kolibakter 0:da5127da2ba0 264
kolibakter 0:da5127da2ba0 265 MOTOR_PID_TICK(0)
kolibakter 0:da5127da2ba0 266 MOTOR_PID_TICK(1)
kolibakter 0:da5127da2ba0 267 MOTOR_PID_TICK(2)
kolibakter 0:da5127da2ba0 268 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 269 MOTOR_PID_TICK(3)
kolibakter 0:da5127da2ba0 270 #endif