Code for TÜ mbed mainboard

Dependencies:   USBDevice mbed motor

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "pins.h"
00003 #include "motor.h"
00004 #include "definitions.h"
00005 #include "USBSerial.h"
00006 
00007 typedef void (*VoidArray) ();
00008 
00009 DigitalOut led(LED1);
00010 DigitalOut l(LED2);
00011 USBSerial pc;
00012 
00013 Ticker motorPidTicker[NUMBER_OF_MOTORS];
00014 
00015 char buf[16];
00016 bool serialData = false;
00017 int serialCount = 0;
00018 
00019 volatile int16_t motorTicks[NUMBER_OF_MOTORS];
00020 volatile uint8_t motorEncNow[NUMBER_OF_MOTORS];
00021 volatile uint8_t motorEncLast[NUMBER_OF_MOTORS];
00022 
00023 Motor motors[NUMBER_OF_MOTORS];
00024 
00025 void serialInterrupt();
00026 void parseCommad(char *command);
00027 
00028 void motor0EncTick();
00029 void motor1EncTick();
00030 void motor2EncTick();
00031 #if NUMBER_OF_MOTORS == 4
00032 void motor3EncTick();
00033 #endif
00034 
00035 void motor0PidTick();
00036 void motor1PidTick();
00037 void motor2PidTick();
00038 #if NUMBER_OF_MOTORS == 4
00039 void motor3PidTick();
00040 #endif
00041 
00042 int main() {
00043     void (*encTicker[])()  = {
00044         motor0EncTick,
00045         motor1EncTick,
00046         motor2EncTick,
00047         #if NUMBER_OF_MOTORS == 4
00048         motor3EncTick
00049         #endif
00050     };
00051 
00052     VoidArray pidTicker[] = {
00053         motor0PidTick,
00054         motor1PidTick,
00055         motor2PidTick,
00056         #if NUMBER_OF_MOTORS == 4
00057         motor3PidTick
00058         #endif
00059     };
00060 
00061     for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
00062         MotorEncA[i]->mode(PullNone);
00063         MotorEncB[i]->mode(PullNone);
00064 
00065         motors[i] = Motor(&pc, MotorPwm[i], MotorDir1[i], MotorDir2[i], MotorFault[i]);
00066 
00067         motorTicks[i] = 0;
00068         motorEncNow[i] = 0;
00069         motorEncLast[i] = 0;
00070 
00071         MotorEncA[i]->rise(encTicker[i]);
00072         MotorEncA[i]->fall(encTicker[i]);
00073         MotorEncB[i]->rise(encTicker[i]);
00074         MotorEncB[i]->fall(encTicker[i]);
00075 
00076         motorPidTicker[i].attach(pidTicker[i], 0.1);
00077 
00078         motors[i].init();
00079     }
00080 
00081     pc.printf("Start\n");
00082 
00083     pc.attach(&serialInterrupt);
00084     int count = 0;
00085     while(1) {
00086         if (count % 20 == 0) {
00087             for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
00088                 pc.printf("s%d:%d\n", i, motors[i].getSpeed());
00089             }
00090         }
00091         if (serialData) {
00092             char temp[16];
00093             memcpy(temp, buf, 16);
00094             memset(buf, 0, 16);
00095             serialData = false;
00096             parseCommad(temp);
00097         }
00098         //motors[0].pid(motor0Ticks);
00099         //motor0Ticks = 0;
00100         wait_ms(50);
00101         count++;
00102 
00103         //pc.printf("buf: %s\n", buf);
00104         //pc.printf("Loop\n");
00105     }
00106 }
00107 
00108 void serialInterrupt(){
00109     while(pc.readable()) {
00110         buf[serialCount] = pc.getc();
00111         serialCount++;
00112     }
00113     if (buf[serialCount - 1] == '\n') {
00114         serialData = true;
00115         serialCount = 0;
00116     }
00117 }
00118 
00119 void parseCommad (char *command) {
00120     if (command[0] == 's' && command[1] == 'd') {
00121         int16_t speed = atoi(command + 2);
00122         motors[0].pid_on = 1;
00123         motors[0].setSpeed(speed);
00124     }
00125     if (command[0] == 's') {
00126         for (int i = 0; i < NUMBER_OF_MOTORS; i++) {
00127             pc.printf("s%d:%d\n", i, motors[i].getSpeed());
00128         }
00129     } else if (command[0] == 'w' && command[1] == 'l') {
00130         int16_t speed = atoi(command + 2);
00131         motors[0].pid_on = 0;
00132         if (speed < 0) motors[0].backward(-1*speed/255.0);
00133         else motors[0].forward(speed/255.0);
00134     } else if (command[0] == 'p' && command[1] == 'p') {
00135         uint8_t pGain = atoi(command + 2);
00136         motors[0].pgain = pGain;
00137     } else if (command[0] == 'p' && command[1] == 'i') {
00138         uint8_t iGain = atoi(command + 2);
00139         motors[0].igain = iGain;
00140     } else if (command[0] == 'p' && command[1] == 'd') {
00141         uint8_t dGain = atoi(command + 2);
00142         motors[0].dgain = dGain;
00143     } else if (command[0] == 'p') {
00144         char gain[20];
00145         motors[0].getPIDGain(gain);
00146         pc.printf("%s\n", gain);
00147     }
00148 }
00149 
00150 MOTOR_ENC_TICK(0)
00151 MOTOR_ENC_TICK(1)
00152 MOTOR_ENC_TICK(2)
00153 #if NUMBER_OF_MOTORS == 4
00154 MOTOR_ENC_TICK(3)
00155 #endif
00156 
00157 MOTOR_PID_TICK(0)
00158 MOTOR_PID_TICK(1)
00159 MOTOR_PID_TICK(2)
00160 #if NUMBER_OF_MOTORS == 4
00161 MOTOR_PID_TICK(3)
00162 #endif