Mihkel Vallap / Mbed 2 deprecated mbed_mainboard_source

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 Karl Oskar Lember

main.cpp@2:31a7326e182e, 2016-11-10 (annotated)

Committer:
Mihkelval
Date:
Thu Nov 10 13:42:20 2016 +0000
Revision:
2:31a7326e182e
Parent:
0:da5127da2ba0
Child:
3:be341df4265f
a

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