Kaarel Adamson / MCH-united

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

Zlatan-mbed/main.cpp@1:110cb8bdfb71, 2017-01-04 (annotated)

Committer:
adamsonk
Date:
Wed Jan 04 10:28:53 2017 +0000
Revision:
1:110cb8bdfb71
Mainboardi kood viki jaoks

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adamsonk 1:110cb8bdfb71 1 #include "mbed.h"
adamsonk 1:110cb8bdfb71 2 #include "pins.h"
adamsonk 1:110cb8bdfb71 3 #include "motor.h"
adamsonk 1:110cb8bdfb71 4 #include "definitions.h"
adamsonk 1:110cb8bdfb71 5 #include "USBSerial.h"
adamsonk 1:110cb8bdfb71 6
adamsonk 1:110cb8bdfb71 7 typedef void (*VoidArray) ();
adamsonk 1:110cb8bdfb71 8
adamsonk 1:110cb8bdfb71 9
adamsonk 1:110cb8bdfb71 10 DigitalOut led(LED1);
adamsonk 1:110cb8bdfb71 11 DigitalOut l(LED2);
adamsonk 1:110cb8bdfb71 12 USBSerial pc;
adamsonk 1:110cb8bdfb71 13
adamsonk 1:110cb8bdfb71 14 Ticker motorPidTicker[NUMBER_OF_MOTORS];
adamsonk 1:110cb8bdfb71 15
adamsonk 1:110cb8bdfb71 16 bool coilChargeBool = false;
adamsonk 1:110cb8bdfb71 17 char buf[16];
adamsonk 1:110cb8bdfb71 18 bool serialData = false;
adamsonk 1:110cb8bdfb71 19 int serialCount = 0;
adamsonk 1:110cb8bdfb71 20 int chargeBool = 0;
adamsonk 1:110cb8bdfb71 21 int counter = 0;
adamsonk 1:110cb8bdfb71 22
adamsonk 1:110cb8bdfb71 23 volatile int16_t motorTicks[NUMBER_OF_MOTORS];
adamsonk 1:110cb8bdfb71 24 volatile uint8_t motorEncNow[NUMBER_OF_MOTORS];
adamsonk 1:110cb8bdfb71 25 volatile uint8_t motorEncLast[NUMBER_OF_MOTORS];
adamsonk 1:110cb8bdfb71 26
adamsonk 1:110cb8bdfb71 27 Motor motors[NUMBER_OF_MOTORS];
adamsonk 1:110cb8bdfb71 28
adamsonk 1:110cb8bdfb71 29 void serialInterrupt();
adamsonk 1:110cb8bdfb71 30 void parseCommand (char *command);
adamsonk 1:110cb8bdfb71 31 void parseString (char *str);
adamsonk 1:110cb8bdfb71 32
adamsonk 1:110cb8bdfb71 33 void motor0EncTick();
adamsonk 1:110cb8bdfb71 34 void motor1EncTick();
adamsonk 1:110cb8bdfb71 35 void motor2EncTick();
adamsonk 1:110cb8bdfb71 36
adamsonk 1:110cb8bdfb71 37 void motor0PidTick();
adamsonk 1:110cb8bdfb71 38 void motor1PidTick();
adamsonk 1:110cb8bdfb71 39 void motor2PidTick();
adamsonk 1:110cb8bdfb71 40
adamsonk 1:110cb8bdfb71 41 void kickBall();
adamsonk 1:110cb8bdfb71 42 void dribblerOn();
adamsonk 1:110cb8bdfb71 43 void dribblerOff();
adamsonk 1:110cb8bdfb71 44 void coilCharge();
adamsonk 1:110cb8bdfb71 45
adamsonk 1:110cb8bdfb71 46
adamsonk 1:110cb8bdfb71 47 int main() {
adamsonk 1:110cb8bdfb71 48 void (*encTicker[])() = {
adamsonk 1:110cb8bdfb71 49 motor0EncTick,
adamsonk 1:110cb8bdfb71 50 motor1EncTick,
adamsonk 1:110cb8bdfb71 51 motor2EncTick
adamsonk 1:110cb8bdfb71 52 };
adamsonk 1:110cb8bdfb71 53
adamsonk 1:110cb8bdfb71 54 VoidArray pidTicker[] = {
adamsonk 1:110cb8bdfb71 55 motor0PidTick,
adamsonk 1:110cb8bdfb71 56 motor1PidTick,
adamsonk 1:110cb8bdfb71 57 motor2PidTick
adamsonk 1:110cb8bdfb71 58 };
adamsonk 1:110cb8bdfb71 59
adamsonk 1:110cb8bdfb71 60 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
adamsonk 1:110cb8bdfb71 61 MotorEncA[i]->mode(PullNone);
adamsonk 1:110cb8bdfb71 62 MotorEncB[i]->mode(PullNone);
adamsonk 1:110cb8bdfb71 63
adamsonk 1:110cb8bdfb71 64 motors[i] = Motor(&pc, MotorPwm[i], MotorDir1[i], MotorDir2[i], MotorFault[i]);
adamsonk 1:110cb8bdfb71 65
adamsonk 1:110cb8bdfb71 66 motorTicks[i] = 0;
adamsonk 1:110cb8bdfb71 67 motorEncNow[i] = 0;
adamsonk 1:110cb8bdfb71 68 motorEncLast[i] = 0;
adamsonk 1:110cb8bdfb71 69
adamsonk 1:110cb8bdfb71 70 MotorEncA[i]->rise(encTicker[i]);
adamsonk 1:110cb8bdfb71 71 MotorEncA[i]->fall(encTicker[i]);
adamsonk 1:110cb8bdfb71 72 MotorEncB[i]->rise(encTicker[i]);
adamsonk 1:110cb8bdfb71 73 MotorEncB[i]->fall(encTicker[i]);
adamsonk 1:110cb8bdfb71 74
adamsonk 1:110cb8bdfb71 75 motorPidTicker[i].attach(pidTicker[i], 0.1);
adamsonk 1:110cb8bdfb71 76
adamsonk 1:110cb8bdfb71 77 motors[i].init();
adamsonk 1:110cb8bdfb71 78 }
adamsonk 1:110cb8bdfb71 79
adamsonk 1:110cb8bdfb71 80
adamsonk 1:110cb8bdfb71 81 KICK = 0;
adamsonk 1:110cb8bdfb71 82 CHARGE = 0;
adamsonk 1:110cb8bdfb71 83
adamsonk 1:110cb8bdfb71 84
adamsonk 1:110cb8bdfb71 85 wait_ms(1500);
adamsonk 1:110cb8bdfb71 86 DRIBBLER=70/1000.0;
adamsonk 1:110cb8bdfb71 87 wait_ms(1500);
adamsonk 1:110cb8bdfb71 88 DRIBBLER=10/1000.0;
adamsonk 1:110cb8bdfb71 89 wait_ms(2000);
adamsonk 1:110cb8bdfb71 90
adamsonk 1:110cb8bdfb71 91
adamsonk 1:110cb8bdfb71 92 pc.printf("Start\n");
adamsonk 1:110cb8bdfb71 93
adamsonk 1:110cb8bdfb71 94 pc.attach(&serialInterrupt);
adamsonk 1:110cb8bdfb71 95
adamsonk 1:110cb8bdfb71 96 while(1) {
adamsonk 1:110cb8bdfb71 97
adamsonk 1:110cb8bdfb71 98 if (serialData) {
adamsonk 1:110cb8bdfb71 99 char temp[32];
adamsonk 1:110cb8bdfb71 100 memcpy(temp, buf, 32);
adamsonk 1:110cb8bdfb71 101 memset(buf, 0, 32);
adamsonk 1:110cb8bdfb71 102 serialData = false;
adamsonk 1:110cb8bdfb71 103 parseString(temp);
adamsonk 1:110cb8bdfb71 104 }
adamsonk 1:110cb8bdfb71 105 if (coilChargeBool) {
adamsonk 1:110cb8bdfb71 106 if ((DONE!=1)){
adamsonk 1:110cb8bdfb71 107 coilChargeBool = false;
adamsonk 1:110cb8bdfb71 108 CHARGE = 0;
adamsonk 1:110cb8bdfb71 109 dribblerOn();
adamsonk 1:110cb8bdfb71 110 pc.printf("CHARGEDD\n");
adamsonk 1:110cb8bdfb71 111 }
adamsonk 1:110cb8bdfb71 112 if (counter>500000){
adamsonk 1:110cb8bdfb71 113 coilChargeBool = false;
adamsonk 1:110cb8bdfb71 114 CHARGE = 0;
adamsonk 1:110cb8bdfb71 115 dribblerOn();
adamsonk 1:110cb8bdfb71 116 pc.printf("CHARGEDT\n");
adamsonk 1:110cb8bdfb71 117 }
adamsonk 1:110cb8bdfb71 118
adamsonk 1:110cb8bdfb71 119 counter++;
adamsonk 1:110cb8bdfb71 120 }
adamsonk 1:110cb8bdfb71 121 wait_ms(20);
adamsonk 1:110cb8bdfb71 122 }
adamsonk 1:110cb8bdfb71 123 }
adamsonk 1:110cb8bdfb71 124
adamsonk 1:110cb8bdfb71 125 void serialInterrupt(){
adamsonk 1:110cb8bdfb71 126 while(pc.readable()) {
adamsonk 1:110cb8bdfb71 127 buf[serialCount] = pc.getc();
adamsonk 1:110cb8bdfb71 128 serialCount++;
adamsonk 1:110cb8bdfb71 129 }
adamsonk 1:110cb8bdfb71 130 if (buf[serialCount - 1] == '\n') {
adamsonk 1:110cb8bdfb71 131 serialData = true;
adamsonk 1:110cb8bdfb71 132 serialCount = 0;
adamsonk 1:110cb8bdfb71 133 }
adamsonk 1:110cb8bdfb71 134 }
adamsonk 1:110cb8bdfb71 135
adamsonk 1:110cb8bdfb71 136 void parseString (char *str) {
adamsonk 1:110cb8bdfb71 137 uint8_t x;
adamsonk 1:110cb8bdfb71 138 size_t i = 1;
adamsonk 1:110cb8bdfb71 139 if (str[0] == '<') {
adamsonk 1:110cb8bdfb71 140 char command[300];
adamsonk 1:110cb8bdfb71 141
adamsonk 1:110cb8bdfb71 142 while (1){
adamsonk 1:110cb8bdfb71 143 x = 0;
adamsonk 1:110cb8bdfb71 144 while(str[i] != ';' && str[i] != '>') {
adamsonk 1:110cb8bdfb71 145 command[x++] = str[i++];
adamsonk 1:110cb8bdfb71 146 }
adamsonk 1:110cb8bdfb71 147 command[x] = '\0';
adamsonk 1:110cb8bdfb71 148 parseCommand(command);
adamsonk 1:110cb8bdfb71 149 if (str[i] == '>') {
adamsonk 1:110cb8bdfb71 150 break;
adamsonk 1:110cb8bdfb71 151 }
adamsonk 1:110cb8bdfb71 152 i++;
adamsonk 1:110cb8bdfb71 153 }
adamsonk 1:110cb8bdfb71 154 }
adamsonk 1:110cb8bdfb71 155 }
adamsonk 1:110cb8bdfb71 156
adamsonk 1:110cb8bdfb71 157
adamsonk 1:110cb8bdfb71 158
adamsonk 1:110cb8bdfb71 159 void parseCommand (char *command) {
adamsonk 1:110cb8bdfb71 160
adamsonk 1:110cb8bdfb71 161 //MOTOR
adamsonk 1:110cb8bdfb71 162 size_t motor;
adamsonk 1:110cb8bdfb71 163 if (command[0] == '1') motor = 0;
adamsonk 1:110cb8bdfb71 164 else if (command[0] == '2') motor = 1;
adamsonk 1:110cb8bdfb71 165 else motor = 2;
adamsonk 1:110cb8bdfb71 166
adamsonk 1:110cb8bdfb71 167
adamsonk 1:110cb8bdfb71 168 if (command[0] == 'd' && command[1] == 'o') {
adamsonk 1:110cb8bdfb71 169 int16_t speed = atoi(command + 3);
adamsonk 1:110cb8bdfb71 170 DRIBBLER = speed/1000.0;
adamsonk 1:110cb8bdfb71 171 }
adamsonk 1:110cb8bdfb71 172
adamsonk 1:110cb8bdfb71 173 if (command[0] == 'd' && command[1] == 'n') {
adamsonk 1:110cb8bdfb71 174 dribblerOn();
adamsonk 1:110cb8bdfb71 175 }
adamsonk 1:110cb8bdfb71 176
adamsonk 1:110cb8bdfb71 177 if (command[0] == 'd' && command[1] == 'f') {
adamsonk 1:110cb8bdfb71 178 dribblerOff();
adamsonk 1:110cb8bdfb71 179 }
adamsonk 1:110cb8bdfb71 180
adamsonk 1:110cb8bdfb71 181 if (command[0] == 'c' && command[1] == 'c') {
adamsonk 1:110cb8bdfb71 182 dribblerOff();
adamsonk 1:110cb8bdfb71 183 coilCharge();
adamsonk 1:110cb8bdfb71 184 }
adamsonk 1:110cb8bdfb71 185
adamsonk 1:110cb8bdfb71 186 if (command[0] == 'c' && command[1] == 'k') {
adamsonk 1:110cb8bdfb71 187 CHARGE = 0;
adamsonk 1:110cb8bdfb71 188 kickBall();
adamsonk 1:110cb8bdfb71 189 dribblerOff();
adamsonk 1:110cb8bdfb71 190 coilCharge();
adamsonk 1:110cb8bdfb71 191 }
adamsonk 1:110cb8bdfb71 192
adamsonk 1:110cb8bdfb71 193 if (command[0] == 'c' && command[1] == 'e') {
adamsonk 1:110cb8bdfb71 194 CHARGE = 0;
adamsonk 1:110cb8bdfb71 195 kickBall();
adamsonk 1:110cb8bdfb71 196 kickBall();
adamsonk 1:110cb8bdfb71 197 kickBall();
adamsonk 1:110cb8bdfb71 198 }
adamsonk 1:110cb8bdfb71 199
adamsonk 1:110cb8bdfb71 200
adamsonk 1:110cb8bdfb71 201 if (command[0] == 'b' && command[1] == 'd') {
adamsonk 1:110cb8bdfb71 202 int n = BALL_DETECT.read();
adamsonk 1:110cb8bdfb71 203 pc.printf("%d\n", n);
adamsonk 1:110cb8bdfb71 204 }
adamsonk 1:110cb8bdfb71 205
adamsonk 1:110cb8bdfb71 206 if (command[0] == 's') {
adamsonk 1:110cb8bdfb71 207 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
adamsonk 1:110cb8bdfb71 208 pc.printf("s%d:%d\n", motors[0].getSpeed());
adamsonk 1:110cb8bdfb71 209 pc.printf("s%d:%d\n", i, motors[i].getSpeed());
adamsonk 1:110cb8bdfb71 210 }
adamsonk 1:110cb8bdfb71 211
adamsonk 1:110cb8bdfb71 212 }
adamsonk 1:110cb8bdfb71 213 if (command[1] == 'w' && command[2] == 'l') {
adamsonk 1:110cb8bdfb71 214 int16_t speed = atoi(command + 3);
adamsonk 1:110cb8bdfb71 215 if (motor==0){
adamsonk 1:110cb8bdfb71 216 motors[0].pid_on = 0;
adamsonk 1:110cb8bdfb71 217 if (speed < 0) motors[0].backward(-1*speed/255.0);
adamsonk 1:110cb8bdfb71 218 else motors[0].forward(speed/255.0);
adamsonk 1:110cb8bdfb71 219 }
adamsonk 1:110cb8bdfb71 220 else{
adamsonk 1:110cb8bdfb71 221 motors[motor].pid_on = 0;
adamsonk 1:110cb8bdfb71 222 if (speed < 0) motors[motor].backward(-1*speed/255.0);
adamsonk 1:110cb8bdfb71 223 else motors[motor].forward(speed/255.0);
adamsonk 1:110cb8bdfb71 224 }
adamsonk 1:110cb8bdfb71 225 }
adamsonk 1:110cb8bdfb71 226 }
adamsonk 1:110cb8bdfb71 227
adamsonk 1:110cb8bdfb71 228 void kickBall(){
adamsonk 1:110cb8bdfb71 229 KICK = 1;
adamsonk 1:110cb8bdfb71 230 pc.printf("KICK\n");
adamsonk 1:110cb8bdfb71 231 wait_ms(5);
adamsonk 1:110cb8bdfb71 232 KICK = 0;
adamsonk 1:110cb8bdfb71 233 }
adamsonk 1:110cb8bdfb71 234
adamsonk 1:110cb8bdfb71 235 void coilCharge(){
adamsonk 1:110cb8bdfb71 236 KICK = 0;
adamsonk 1:110cb8bdfb71 237 CHARGE = 1;
adamsonk 1:110cb8bdfb71 238 counter = 0;
adamsonk 1:110cb8bdfb71 239 coilChargeBool = true;
adamsonk 1:110cb8bdfb71 240 pc.printf("Charge-in\n");
adamsonk 1:110cb8bdfb71 241 }
adamsonk 1:110cb8bdfb71 242
adamsonk 1:110cb8bdfb71 243
adamsonk 1:110cb8bdfb71 244 void dribblerOn(){
adamsonk 1:110cb8bdfb71 245 DRIBBLER = 58/1000.0;
adamsonk 1:110cb8bdfb71 246 }
adamsonk 1:110cb8bdfb71 247
adamsonk 1:110cb8bdfb71 248 void dribblerOff(){
adamsonk 1:110cb8bdfb71 249 DRIBBLER = 51/1000.0;
adamsonk 1:110cb8bdfb71 250 }
adamsonk 1:110cb8bdfb71 251
adamsonk 1:110cb8bdfb71 252
adamsonk 1:110cb8bdfb71 253 MOTOR_ENC_TICK(0)
adamsonk 1:110cb8bdfb71 254 MOTOR_ENC_TICK(1)
adamsonk 1:110cb8bdfb71 255 MOTOR_ENC_TICK(2)
adamsonk 1:110cb8bdfb71 256
adamsonk 1:110cb8bdfb71 257
adamsonk 1:110cb8bdfb71 258 MOTOR_PID_TICK(0)
adamsonk 1:110cb8bdfb71 259 MOTOR_PID_TICK(1)
adamsonk 1:110cb8bdfb71 260 MOTOR_PID_TICK(2)