Mihkel Vallap / Mbed 2 deprecated mbed_mainboard_source

Dependencies:   USBDevice mbed motor

Fork of mbed_mainboard_source by Karl Oskar Lember

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?

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
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