mit
Diff: main.cpp
- Revision:
- 4:5ae9f8b3a16f
- Parent:
- 3:cae0b305d54c
- Child:
- 5:e90c8b57811c
--- a/main.cpp Fri Jul 12 00:13:37 2013 +0000 +++ b/main.cpp Tue Nov 24 03:56:22 2015 +0000 @@ -1,227 +1,104 @@ //Ben Katz, 2013 - -#include "QEI.h" +//Austin Buchan, 2015 +#include "mbed.h" +#include "Motor.h" #include "PID.h" -#define pi 3.14159265358979323846 - - -Serial pc(USBTX, USBRX); - -//Use X4 encoding. -QEI wheel(p13, p14, NC, 1200, QEI::X4_ENCODING); //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2)) - -AnalogIn p_pot (p15); -AnalogIn d_pot (p16); -AnalogIn a_pot (p17); - -PwmOut Motor1(p21); //Ouput pin to h-bridge 1 -PwmOut Motor2 (p22); //Output pin to h-bridge 2 -AnalogIn Current (p20); //Current sense pin - -int steps_per_rev = 1200; //Encoder CPR * gearbox ratio -Timer timer; - - +#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 -float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians -{ - return ((pulses/steps_per_rev)*(2*pi)); -} - -float pulsesToDegrees(float pulses) //Encoder steps to revolutions of output shaft in degrees -{ - - return ((pulses/steps_per_rev)*360); -} - +#define UART_RX_PIN PTB1 +#define UART_TX_PIN PTB2 -void signal_to_hbridge( float signal) //Input of -1 means full reverse, 1 means full forward -{ //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write() function - if (signal < 0) { - Motor1.write(0); - Motor2.write(signal * -1); - } else if (signal > 0) { - Motor1.write(signal); - Motor2.write(0); - } else { - Motor1.write(0); - Motor2.write(0); - } -} +#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 - - -void pulse_motor(float pulse_time, float pulse_interval) //Usefull for testing peak torque. - //pulse_time = motor on time. pulse_interval = motor off time -{ +#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 - timer.start(); - while (timer.read() < pulse_time){ - Motor1.write(1); - Motor2.write(0); - //Prints current draw to PC - printf("%F", (Current.read())*3.3*4/.525); //Arithmetic converts the analog input to Amps - printf("\n"); //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A) - - } - timer.stop(); - timer.reset(); - wait(pulse_time); - Motor1.write(0); - Motor2.write(0); - wait(pulse_interval); -} - +Serial pc(UART_TX_PIN, UART_RX_PIN); - -int main() -{ +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 +); - Motor1.period(.00005); //Set PWM frequency. MC33926 h-bridge limited 20 kHz (.00005 seconds) - Motor2.period(.00005); - PIDController controller(40, .1, 0.1, 3, 0, 0.0007, 0.0000001); //(desired position, torque, position P gain, position D gain, position I gain, torque P gain, torque I gain) - //timer.start(); - //float currentSum = 0; - //float angleSum = 0; - - wait(.7); - - wait(.5); - printf("%F", pulsesToDegrees(wheel.getPulses())); - printf("\n\r"); - while(1){ - signal_to_hbridge(controller.command_position_tm()); - //printf("%F", pulsesToDegrees(wheel.getPulses())); - } - - - +//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(); } - - -PIDController::PIDController(float desired_position, float desired_torque, float p_gain_p, float d_gain_p, float i_gain_p, float p_gain_c, float i_gain_c) -{ - kp_p = p_gain_p; - kd_p = d_gain_p; - ki_p = i_gain_p; - kp_c = p_gain_c; - ki_c = i_gain_c; - torque = desired_torque; - current_position = 0; - torque_command = 0; - c_torque = 0; - error = 0; - old_error = 0; - error_sum = 0; - goal_position = desired_position; - direction = 0; - counter = 0; - timer.start(); - command = 0; -} +Ticker position_ticker; -//voltage mode position control -float PIDController::command_position(void) //This function is called to set the desired position of the servo -{ - // wait(.0004); //This delay allows enough time to register a position difference between cycles. - //Without the delay, velocity is read as 0, so there is no D feedback. - //The delay can be decreased when adding more servos, as the computation time becomes more significant. - - float desired_position = this->goal_position; //All value are stored in the PIDController object created, and can be changed at any time - float p_gain = this->kp_p; - float d_gain = this->kd_p; - float i_gain = this->ki_p; - if (this->timer.read_us() > 400){ - float currentPosition = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians - - this->error = (currentPosition - desired_position); - this->integral_error += this->error; - - this->command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error); - - this->old_error = error; - } - - return this->command; - +void update_position(void) { + motor.set_command(position_controller.command_position(motor.get_position())); } +DigitalOut led1(LED1); +DigitalOut led2(LED2); +DigitalOut led3(LED3); -float PIDController::command_position_tm(void){ //Torque Mode position control - //wait(.0003); - ///* - +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; - // */ - //this->current_position = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians - float desired_position = this->goal_position; - float p_gain = this->kp_p; - float d_gain = this->kd_p; - float i_gain = this->ki_p; - if (this->timer.read_us() > 400){ - this->current_position = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians - this->old_error = this->error; - this->error = (this->current_position - desired_position); - this-> command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error); - this->integral_error += this->error; - this->torque_command = command; - this->timer.reset(); - } + led3 = 0; + printf("Starting\n\r"); - - - - return this->command_torque(); - - -} - - - -float PIDController::command_torque(void){ - //wait(.0004); - float current_torque; - - if (this->direction != 0){ - current_torque = this->direction*(Current.read()*3.3/.525); + 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); } - else{ - current_torque = (Current.read()*3.3/.525); - } - float average = 0; - - for (int i = 0; i < 4; i++){ - this->past_currents[i] = this->past_currents[i+1]; - average += past_currents[i]; - } - average += current_torque; - average = average/5; - average = current_torque; - this->past_currents[4] = current_torque; - - this->c_torque = average; - this->c_error = (this->torque - average); - - this->error_sum += this->c_error; - - this->torque_command += -1*(this->kp_c*this->c_error + this->ki_c*this->error_sum); - if (this->torque_command > 0){ - this->direction = -1; - } - else if(this->torque_command < 0){ - this->direction = 1; - } - else{ - this->direction = 0; - } - if (this->torque_command > 1){ - this->torque_command = 1; - } - else if (this->torque_command < -1){ - this->torque_command = -1; - } - - return this->torque_command; -} +} \ No newline at end of file