mit
main.cpp
- Committer:
- abuchan
- Date:
- 2015-11-24
- Revision:
- 4:5ae9f8b3a16f
- Parent:
- 3:cae0b305d54c
- Child:
- 5:e90c8b57811c
File content as of revision 4:5ae9f8b3a16f:
//Ben Katz, 2013 //Austin Buchan, 2015 #include "mbed.h" #include "Motor.h" #include "PID.h" #define PWM_PIN PTB13 #define MIN_1_PIN PTA0 #define MIN_2_PIN PTA8 #define ENC_A_PIN PTB6 #define ENC_B_PIN PTB7 #define CURRENT_PIN PTA9 #define UART_RX_PIN PTB1 #define UART_TX_PIN PTB2 #define PWM_PERIOD 0.001 #define PID_CUR_PERIOD 0.001 #define PID_POS_PERIOD 0.005 #define PID_POS_P_GAIN 0.5 #define PID_POS_D_GAIN 10.0 #define PID_POS_I_GAIN 0.1 #define PID_CUR_P_GAIN 0.5 #define PID_CUR_D_GAIN 0.0 //#define PID_CUR_I_GAIN 0.001 #define PID_CUR_I_GAIN 0.01 Serial pc(UART_TX_PIN, UART_RX_PIN); Motor motor( PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD, CURRENT_PIN, ENC_A_PIN, ENC_B_PIN, PID_CUR_P_GAIN, PID_CUR_D_GAIN, PID_CUR_I_GAIN ); //HBridge hbridge(PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD); //CurrentSense current_sense(CURRENT_PIN); //Encoder encoder(ENC_A_PIN, ENC_B_PIN); PIDController position_controller( PID_POS_P_GAIN, PID_POS_D_GAIN, PID_POS_I_GAIN ); Ticker current_ticker; void update_current(void) { motor.update(); } Ticker position_ticker; void update_position(void) { motor.set_command(position_controller.command_position(motor.get_position())); } DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); int main() { printf("Built " __DATE__ " " __TIME__ "\r\n"); led1 = 1; led2 = 1; led3 = 1; wait(.5); led2 = 0; printf("Initializing\n\r"); current_ticker.attach(&update_current, PID_CUR_PERIOD); position_ticker.attach(&update_position, PID_POS_PERIOD); int count = 0; float command = 0.0; led3 = 0; printf("Starting\n\r"); while(1){ //command = 0.05 * abs((50-count)/10) - 0.1; //motor.set_command(command); command = 1.0 * abs((55-count)/10) - 0.25; position_controller.set_command(command); led1 = (count % 2); //printf("C:%F Cu:%F P:%F\n\r", command, current_sense.get_current(), encoder.get_position()); //printf("PC:%F\n\r", position_controller.command); printf("MP:%F MV:%F MCu:%F MCo:%F MO:%F MT:%F P:%F\n\r", motor.position, motor.velocity, motor.current, motor.command, motor.output, motor.torque, position_controller.command ); count = (count + 1) % 100; wait(0.1); } }