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
main.cpp@0:da5127da2ba0, 2016-10-13 (annotated)
- Committer:
- kolibakter
- Date:
- Thu Oct 13 15:25:01 2016 +0000
- Revision:
- 0:da5127da2ba0
- Child:
- 2:31a7326e182e
Added copy from https://github.com/jaantti/Mainboard-mbed-source. Made some changes to serial comminucation.
Who changed what in which revision?
User | Revision | Line number | New 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 | |
kolibakter | 0:da5127da2ba0 | 9 | DigitalOut led(LED1); |
kolibakter | 0:da5127da2ba0 | 10 | DigitalOut l(LED2); |
kolibakter | 0:da5127da2ba0 | 11 | USBSerial pc; |
kolibakter | 0:da5127da2ba0 | 12 | |
kolibakter | 0:da5127da2ba0 | 13 | Ticker motorPidTicker[NUMBER_OF_MOTORS]; |
kolibakter | 0:da5127da2ba0 | 14 | |
kolibakter | 0:da5127da2ba0 | 15 | char buf[16]; |
kolibakter | 0:da5127da2ba0 | 16 | bool serialData = false; |
kolibakter | 0:da5127da2ba0 | 17 | int serialCount = 0; |
kolibakter | 0:da5127da2ba0 | 18 | |
kolibakter | 0:da5127da2ba0 | 19 | volatile int16_t motorTicks[NUMBER_OF_MOTORS]; |
kolibakter | 0:da5127da2ba0 | 20 | volatile uint8_t motorEncNow[NUMBER_OF_MOTORS]; |
kolibakter | 0:da5127da2ba0 | 21 | volatile uint8_t motorEncLast[NUMBER_OF_MOTORS]; |
kolibakter | 0:da5127da2ba0 | 22 | |
kolibakter | 0:da5127da2ba0 | 23 | Motor motors[NUMBER_OF_MOTORS]; |
kolibakter | 0:da5127da2ba0 | 24 | |
kolibakter | 0:da5127da2ba0 | 25 | void serialInterrupt(); |
kolibakter | 0:da5127da2ba0 | 26 | void parseCommad(char *command); |
kolibakter | 0:da5127da2ba0 | 27 | |
kolibakter | 0:da5127da2ba0 | 28 | void motor0EncTick(); |
kolibakter | 0:da5127da2ba0 | 29 | void motor1EncTick(); |
kolibakter | 0:da5127da2ba0 | 30 | void motor2EncTick(); |
kolibakter | 0:da5127da2ba0 | 31 | #if NUMBER_OF_MOTORS == 4 |
kolibakter | 0:da5127da2ba0 | 32 | void motor3EncTick(); |
kolibakter | 0:da5127da2ba0 | 33 | #endif |
kolibakter | 0:da5127da2ba0 | 34 | |
kolibakter | 0:da5127da2ba0 | 35 | void motor0PidTick(); |
kolibakter | 0:da5127da2ba0 | 36 | void motor1PidTick(); |
kolibakter | 0:da5127da2ba0 | 37 | void motor2PidTick(); |
kolibakter | 0:da5127da2ba0 | 38 | #if NUMBER_OF_MOTORS == 4 |
kolibakter | 0:da5127da2ba0 | 39 | void motor3PidTick(); |
kolibakter | 0:da5127da2ba0 | 40 | #endif |
kolibakter | 0:da5127da2ba0 | 41 | |
kolibakter | 0:da5127da2ba0 | 42 | int main() { |
kolibakter | 0:da5127da2ba0 | 43 | void (*encTicker[])() = { |
kolibakter | 0:da5127da2ba0 | 44 | motor0EncTick, |
kolibakter | 0:da5127da2ba0 | 45 | motor1EncTick, |
kolibakter | 0:da5127da2ba0 | 46 | motor2EncTick, |
kolibakter | 0:da5127da2ba0 | 47 | #if NUMBER_OF_MOTORS == 4 |
kolibakter | 0:da5127da2ba0 | 48 | motor3EncTick |
kolibakter | 0:da5127da2ba0 | 49 | #endif |
kolibakter | 0:da5127da2ba0 | 50 | }; |
kolibakter | 0:da5127da2ba0 | 51 | |
kolibakter | 0:da5127da2ba0 | 52 | VoidArray pidTicker[] = { |
kolibakter | 0:da5127da2ba0 | 53 | motor0PidTick, |
kolibakter | 0:da5127da2ba0 | 54 | motor1PidTick, |
kolibakter | 0:da5127da2ba0 | 55 | motor2PidTick, |
kolibakter | 0:da5127da2ba0 | 56 | #if NUMBER_OF_MOTORS == 4 |
kolibakter | 0:da5127da2ba0 | 57 | motor3PidTick |
kolibakter | 0:da5127da2ba0 | 58 | #endif |
kolibakter | 0:da5127da2ba0 | 59 | }; |
kolibakter | 0:da5127da2ba0 | 60 | |
kolibakter | 0:da5127da2ba0 | 61 | for (int i = 0; i < NUMBER_OF_MOTORS; i++) { |
kolibakter | 0:da5127da2ba0 | 62 | MotorEncA[i]->mode(PullNone); |
kolibakter | 0:da5127da2ba0 | 63 | MotorEncB[i]->mode(PullNone); |
kolibakter | 0:da5127da2ba0 | 64 | |
kolibakter | 0:da5127da2ba0 | 65 | motors[i] = Motor(&pc, MotorPwm[i], MotorDir1[i], MotorDir2[i], MotorFault[i]); |
kolibakter | 0:da5127da2ba0 | 66 | |
kolibakter | 0:da5127da2ba0 | 67 | motorTicks[i] = 0; |
kolibakter | 0:da5127da2ba0 | 68 | motorEncNow[i] = 0; |
kolibakter | 0:da5127da2ba0 | 69 | motorEncLast[i] = 0; |
kolibakter | 0:da5127da2ba0 | 70 | |
kolibakter | 0:da5127da2ba0 | 71 | MotorEncA[i]->rise(encTicker[i]); |
kolibakter | 0:da5127da2ba0 | 72 | MotorEncA[i]->fall(encTicker[i]); |
kolibakter | 0:da5127da2ba0 | 73 | MotorEncB[i]->rise(encTicker[i]); |
kolibakter | 0:da5127da2ba0 | 74 | MotorEncB[i]->fall(encTicker[i]); |
kolibakter | 0:da5127da2ba0 | 75 | |
kolibakter | 0:da5127da2ba0 | 76 | motorPidTicker[i].attach(pidTicker[i], 0.1); |
kolibakter | 0:da5127da2ba0 | 77 | |
kolibakter | 0:da5127da2ba0 | 78 | motors[i].init(); |
kolibakter | 0:da5127da2ba0 | 79 | } |
kolibakter | 0:da5127da2ba0 | 80 | |
kolibakter | 0:da5127da2ba0 | 81 | pc.printf("Start\n"); |
kolibakter | 0:da5127da2ba0 | 82 | |
kolibakter | 0:da5127da2ba0 | 83 | pc.attach(&serialInterrupt); |
kolibakter | 0:da5127da2ba0 | 84 | int count = 0; |
kolibakter | 0:da5127da2ba0 | 85 | while(1) { |
kolibakter | 0:da5127da2ba0 | 86 | if (count % 20 == 0) { |
kolibakter | 0:da5127da2ba0 | 87 | for (int i = 0; i < NUMBER_OF_MOTORS; i++) { |
kolibakter | 0:da5127da2ba0 | 88 | pc.printf("s%d:%d\n", i, motors[i].getSpeed()); |
kolibakter | 0:da5127da2ba0 | 89 | } |
kolibakter | 0:da5127da2ba0 | 90 | } |
kolibakter | 0:da5127da2ba0 | 91 | if (serialData) { |
kolibakter | 0:da5127da2ba0 | 92 | char temp[16]; |
kolibakter | 0:da5127da2ba0 | 93 | memcpy(temp, buf, 16); |
kolibakter | 0:da5127da2ba0 | 94 | memset(buf, 0, 16); |
kolibakter | 0:da5127da2ba0 | 95 | serialData = false; |
kolibakter | 0:da5127da2ba0 | 96 | parseCommad(temp); |
kolibakter | 0:da5127da2ba0 | 97 | } |
kolibakter | 0:da5127da2ba0 | 98 | //motors[0].pid(motor0Ticks); |
kolibakter | 0:da5127da2ba0 | 99 | //motor0Ticks = 0; |
kolibakter | 0:da5127da2ba0 | 100 | wait_ms(50); |
kolibakter | 0:da5127da2ba0 | 101 | count++; |
kolibakter | 0:da5127da2ba0 | 102 | |
kolibakter | 0:da5127da2ba0 | 103 | //pc.printf("buf: %s\n", buf); |
kolibakter | 0:da5127da2ba0 | 104 | //pc.printf("Loop\n"); |
kolibakter | 0:da5127da2ba0 | 105 | } |
kolibakter | 0:da5127da2ba0 | 106 | } |
kolibakter | 0:da5127da2ba0 | 107 | |
kolibakter | 0:da5127da2ba0 | 108 | void serialInterrupt(){ |
kolibakter | 0:da5127da2ba0 | 109 | while(pc.readable()) { |
kolibakter | 0:da5127da2ba0 | 110 | buf[serialCount] = pc.getc(); |
kolibakter | 0:da5127da2ba0 | 111 | serialCount++; |
kolibakter | 0:da5127da2ba0 | 112 | } |
kolibakter | 0:da5127da2ba0 | 113 | if (buf[serialCount - 1] == '\n') { |
kolibakter | 0:da5127da2ba0 | 114 | serialData = true; |
kolibakter | 0:da5127da2ba0 | 115 | serialCount = 0; |
kolibakter | 0:da5127da2ba0 | 116 | } |
kolibakter | 0:da5127da2ba0 | 117 | } |
kolibakter | 0:da5127da2ba0 | 118 | |
kolibakter | 0:da5127da2ba0 | 119 | void parseCommad (char *command) { |
kolibakter | 0:da5127da2ba0 | 120 | if (command[0] == 's' && command[1] == 'd') { |
kolibakter | 0:da5127da2ba0 | 121 | int16_t speed = atoi(command + 2); |
kolibakter | 0:da5127da2ba0 | 122 | motors[0].pid_on = 1; |
kolibakter | 0:da5127da2ba0 | 123 | motors[0].setSpeed(speed); |
kolibakter | 0:da5127da2ba0 | 124 | } |
kolibakter | 0:da5127da2ba0 | 125 | if (command[0] == 's') { |
kolibakter | 0:da5127da2ba0 | 126 | for (int i = 0; i < NUMBER_OF_MOTORS; i++) { |
kolibakter | 0:da5127da2ba0 | 127 | pc.printf("s%d:%d\n", i, motors[i].getSpeed()); |
kolibakter | 0:da5127da2ba0 | 128 | } |
kolibakter | 0:da5127da2ba0 | 129 | } else if (command[0] == 'w' && command[1] == 'l') { |
kolibakter | 0:da5127da2ba0 | 130 | int16_t speed = atoi(command + 2); |
kolibakter | 0:da5127da2ba0 | 131 | motors[0].pid_on = 0; |
kolibakter | 0:da5127da2ba0 | 132 | if (speed < 0) motors[0].backward(-1*speed/255.0); |
kolibakter | 0:da5127da2ba0 | 133 | else motors[0].forward(speed/255.0); |
kolibakter | 0:da5127da2ba0 | 134 | } else if (command[0] == 'p' && command[1] == 'p') { |
kolibakter | 0:da5127da2ba0 | 135 | uint8_t pGain = atoi(command + 2); |
kolibakter | 0:da5127da2ba0 | 136 | motors[0].pgain = pGain; |
kolibakter | 0:da5127da2ba0 | 137 | } else if (command[0] == 'p' && command[1] == 'i') { |
kolibakter | 0:da5127da2ba0 | 138 | uint8_t iGain = atoi(command + 2); |
kolibakter | 0:da5127da2ba0 | 139 | motors[0].igain = iGain; |
kolibakter | 0:da5127da2ba0 | 140 | } else if (command[0] == 'p' && command[1] == 'd') { |
kolibakter | 0:da5127da2ba0 | 141 | uint8_t dGain = atoi(command + 2); |
kolibakter | 0:da5127da2ba0 | 142 | motors[0].dgain = dGain; |
kolibakter | 0:da5127da2ba0 | 143 | } else if (command[0] == 'p') { |
kolibakter | 0:da5127da2ba0 | 144 | char gain[20]; |
kolibakter | 0:da5127da2ba0 | 145 | motors[0].getPIDGain(gain); |
kolibakter | 0:da5127da2ba0 | 146 | pc.printf("%s\n", gain); |
kolibakter | 0:da5127da2ba0 | 147 | } |
kolibakter | 0:da5127da2ba0 | 148 | } |
kolibakter | 0:da5127da2ba0 | 149 | |
kolibakter | 0:da5127da2ba0 | 150 | MOTOR_ENC_TICK(0) |
kolibakter | 0:da5127da2ba0 | 151 | MOTOR_ENC_TICK(1) |
kolibakter | 0:da5127da2ba0 | 152 | MOTOR_ENC_TICK(2) |
kolibakter | 0:da5127da2ba0 | 153 | #if NUMBER_OF_MOTORS == 4 |
kolibakter | 0:da5127da2ba0 | 154 | MOTOR_ENC_TICK(3) |
kolibakter | 0:da5127da2ba0 | 155 | #endif |
kolibakter | 0:da5127da2ba0 | 156 | |
kolibakter | 0:da5127da2ba0 | 157 | MOTOR_PID_TICK(0) |
kolibakter | 0:da5127da2ba0 | 158 | MOTOR_PID_TICK(1) |
kolibakter | 0:da5127da2ba0 | 159 | MOTOR_PID_TICK(2) |
kolibakter | 0:da5127da2ba0 | 160 | #if NUMBER_OF_MOTORS == 4 |
kolibakter | 0:da5127da2ba0 | 161 | MOTOR_PID_TICK(3) |
kolibakter | 0:da5127da2ba0 | 162 | #endif |