Code for TÜ mbed mainboard
Dependencies: USBDevice mbed motor
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
Generated on Sun Jul 17 2022 04:37:01 by
1.7.2