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

Committer:
Mihkelval
Date:
Mon Nov 14 19:43:02 2016 +0000
Revision:
3:be341df4265f
Parent:
2:31a7326e182e 
asd

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 3:be341df4265f 53 int m = 1;
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
kolibakter 0:da5127da2ba0 100 while(1) {
Mihkelval 2:31a7326e182e 101 //if (Done.read() == 1){
Mihkelval 2:31a7326e182e 102 // Charge.write(0);
Mihkelval 2:31a7326e182e 103 // wait_ms(5);
Mihkelval 2:31a7326e182e 104 // Kick.write(1); }
Mihkelval 2:31a7326e182e 105
Mihkelval 2:31a7326e182e 106 if (count % 50 == 0) {
Mihkelval 2:31a7326e182e 107 //for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
Mihkelval 2:31a7326e182e 108 // pc.printf("s%d:%d\n", i, motors[i].getSpeed());
Mihkelval 2:31a7326e182e 109 //}
Mihkelval 2:31a7326e182e 110 pc.printf("%d\n", enable.read());
Mihkelval 2:31a7326e182e 111 //pc.printf("%d\n", Done.read());
Mihkelval 2:31a7326e182e 112
kolibakter 0:da5127da2ba0 113 }
kolibakter 0:da5127da2ba0 114 if (serialData) {
kolibakter 0:da5127da2ba0 115 char temp[16];
kolibakter 0:da5127da2ba0 116 memcpy(temp, buf, 16);
kolibakter 0:da5127da2ba0 117 memset(buf, 0, 16);
kolibakter 0:da5127da2ba0 118 serialData = false;
kolibakter 0:da5127da2ba0 119 parseCommad(temp);
kolibakter 0:da5127da2ba0 120 }
kolibakter 0:da5127da2ba0 121 //motors[0].pid(motor0Ticks);
kolibakter 0:da5127da2ba0 122 //motor0Ticks = 0;
Mihkelval 3:be341df4265f 123 wait_ms(1);
kolibakter 0:da5127da2ba0 124 count++;
kolibakter 0:da5127da2ba0 125 //pc.printf("buf: %s\n", buf);
kolibakter 0:da5127da2ba0 126 //pc.printf("Loop\n");
kolibakter 0:da5127da2ba0 127 }
kolibakter 0:da5127da2ba0 128 }
kolibakter 0:da5127da2ba0 129
kolibakter 0:da5127da2ba0 130 void serialInterrupt(){
kolibakter 0:da5127da2ba0 131 while(pc.readable()) {
kolibakter 0:da5127da2ba0 132 buf[serialCount] = pc.getc();
kolibakter 0:da5127da2ba0 133 serialCount++;
kolibakter 0:da5127da2ba0 134 }
kolibakter 0:da5127da2ba0 135 if (buf[serialCount - 1] == '\n') {
kolibakter 0:da5127da2ba0 136 serialData = true;
kolibakter 0:da5127da2ba0 137 serialCount = 0;
kolibakter 0:da5127da2ba0 138 }
kolibakter 0:da5127da2ba0 139 }
kolibakter 0:da5127da2ba0 140
kolibakter 0:da5127da2ba0 141 void parseCommad (char *command) {
Mihkelval 2:31a7326e182e 142 if (command[0] == 'a' && command[1] == 's' && command[2] == 'd') {
Mihkelval 2:31a7326e182e 143 int16_t speed = atoi(command + 3);
Mihkelval 2:31a7326e182e 144 motors[0].pid_on = 1;
Mihkelval 2:31a7326e182e 145 motors[1].pid_on = 1;
Mihkelval 2:31a7326e182e 146 motors[2].pid_on = 1;
Mihkelval 2:31a7326e182e 147 motors[0].setSpeed(speed);
Mihkelval 2:31a7326e182e 148 motors[1].setSpeed(speed);
Mihkelval 2:31a7326e182e 149 motors[2].setSpeed(0);
Mihkelval 2:31a7326e182e 150 }
Mihkelval 2:31a7326e182e 151 else if (command[0] == 'b' && command[1] == 's' && command[2] == 'd') {
Mihkelval 2:31a7326e182e 152 int16_t speed = atoi(command + 3);
kolibakter 0:da5127da2ba0 153 motors[0].pid_on = 1;
Mihkelval 2:31a7326e182e 154 motors[1].pid_on = 1;
Mihkelval 2:31a7326e182e 155 motors[2].pid_on = 1;
kolibakter 0:da5127da2ba0 156 motors[0].setSpeed(speed);
Mihkelval 2:31a7326e182e 157 motors[1].setSpeed(0);
Mihkelval 2:31a7326e182e 158 motors[2].setSpeed(speed);
Mihkelval 2:31a7326e182e 159 }
Mihkelval 2:31a7326e182e 160 else if (command[0] == 'c' && command[1] == 's' && command[2] == 'd') {
Mihkelval 2:31a7326e182e 161 int16_t speed = atoi(command + 3);
Mihkelval 2:31a7326e182e 162 motors[0].pid_on = 1;
Mihkelval 2:31a7326e182e 163 motors[1].pid_on = 1;
Mihkelval 2:31a7326e182e 164 motors[2].pid_on = 1;
Mihkelval 2:31a7326e182e 165 motors[0].setSpeed(0);
Mihkelval 2:31a7326e182e 166 motors[1].setSpeed(speed * -1);
Mihkelval 2:31a7326e182e 167 motors[2].setSpeed(speed);
kolibakter 0:da5127da2ba0 168 }
kolibakter 0:da5127da2ba0 169 if (command[0] == 's') {
kolibakter 0:da5127da2ba0 170 for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
kolibakter 0:da5127da2ba0 171 pc.printf("s%d:%d\n", i, motors[i].getSpeed());
kolibakter 0:da5127da2ba0 172 }
Mihkelval 3:be341df4265f 173 }
Mihkelval 2:31a7326e182e 174 else if (command[0] == 'p' && command[1] == 'p') {
kolibakter 0:da5127da2ba0 175 uint8_t pGain = atoi(command + 2);
kolibakter 0:da5127da2ba0 176 motors[0].pgain = pGain;
Mihkelval 2:31a7326e182e 177 motors[1].pgain = pGain;
Mihkelval 2:31a7326e182e 178 motors[2].pgain = pGain;
kolibakter 0:da5127da2ba0 179 } else if (command[0] == 'p' && command[1] == 'i') {
kolibakter 0:da5127da2ba0 180 uint8_t iGain = atoi(command + 2);
kolibakter 0:da5127da2ba0 181 motors[0].igain = iGain;
Mihkelval 2:31a7326e182e 182 motors[1].igain = iGain;
Mihkelval 2:31a7326e182e 183 motors[2].igain = iGain;
kolibakter 0:da5127da2ba0 184 } else if (command[0] == 'p' && command[1] == 'd') {
kolibakter 0:da5127da2ba0 185 uint8_t dGain = atoi(command + 2);
kolibakter 0:da5127da2ba0 186 motors[0].dgain = dGain;
Mihkelval 2:31a7326e182e 187 motors[1].dgain = dGain;
Mihkelval 2:31a7326e182e 188 motors[2].dgain = dGain;
kolibakter 0:da5127da2ba0 189 } else if (command[0] == 'p') {
kolibakter 0:da5127da2ba0 190 char gain[20];
kolibakter 0:da5127da2ba0 191 motors[0].getPIDGain(gain);
kolibakter 0:da5127da2ba0 192 pc.printf("%s\n", gain);
kolibakter 0:da5127da2ba0 193 }
Mihkelval 2:31a7326e182e 194 else if (command[0] == 'd') {
Mihkelval 3:be341df4265f 195 int PWM = atoi(command+1);
Mihkelval 3:be341df4265f 196 dribbler.pulsewidth_us(PWM);
Mihkelval 2:31a7326e182e 197 }
Mihkelval 2:31a7326e182e 198 else if (command[0] == 'j') {
Mihkelval 2:31a7326e182e 199 Charge.write(1);
Mihkelval 2:31a7326e182e 200 }
Mihkelval 2:31a7326e182e 201 else if (command[0] == 'l') {
Mihkelval 2:31a7326e182e 202 Charge.write(0);
Mihkelval 2:31a7326e182e 203 wait_ms(1);
Mihkelval 2:31a7326e182e 204 Kick.write(1);
Mihkelval 3:be341df4265f 205 wait_ms(m);
Mihkelval 3:be341df4265f 206 Kick.write(0);
Mihkelval 2:31a7326e182e 207 }
Mihkelval 2:31a7326e182e 208 else if (command[0] == 'k'){
Mihkelval 2:31a7326e182e 209 Kick.write(0);
Mihkelval 2:31a7326e182e 210 }
Mihkelval 3:be341df4265f 211 else if (command[0] == 'm'){
Mihkelval 3:be341df4265f 212 m = atoi(command+1);
Mihkelval 3:be341df4265f 213 }
kolibakter 0:da5127da2ba0 214 }
kolibakter 0:da5127da2ba0 215
kolibakter 0:da5127da2ba0 216 MOTOR_ENC_TICK(0)
kolibakter 0:da5127da2ba0 217 MOTOR_ENC_TICK(1)
kolibakter 0:da5127da2ba0 218 MOTOR_ENC_TICK(2)
kolibakter 0:da5127da2ba0 219 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 220 MOTOR_ENC_TICK(3)
kolibakter 0:da5127da2ba0 221 #endif
kolibakter 0:da5127da2ba0 222
kolibakter 0:da5127da2ba0 223 MOTOR_PID_TICK(0)
kolibakter 0:da5127da2ba0 224 MOTOR_PID_TICK(1)
kolibakter 0:da5127da2ba0 225 MOTOR_PID_TICK(2)
kolibakter 0:da5127da2ba0 226 #if NUMBER_OF_MOTORS == 4
kolibakter 0:da5127da2ba0 227 MOTOR_PID_TICK(3)
kolibakter 0:da5127da2ba0 228 #endif