Decoupled position and current control working.
Revision 4:5ae9f8b3a16f, committed 2015-11-24
- Comitter:
- abuchan
- Date:
- Tue Nov 24 03:56:22 2015 +0000
- Parent:
- 3:cae0b305d54c
- Commit message:
- Decoupled position and current control working.
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CurrentSense.cpp Tue Nov 24 03:56:22 2015 +0000 @@ -0,0 +1,7 @@ +#include "CurrentSense.h" + +CurrentSense::CurrentSense(PinName current_pin): current_pin_(current_pin) {} + +float CurrentSense::get_current(void) { + return ADC_TO_CURRENT_SCALE * current_pin_.read(); +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CurrentSense.h Tue Nov 24 03:56:22 2015 +0000 @@ -0,0 +1,19 @@ +#include "mbed.h" + +#ifndef CURRENT_SENSE_H +#define CURRENT_SENSE_H + +// 3.3V max ADC reading / 0.525V/A from the current amp +#define ADC_TO_CURRENT_SCALE (3.3/.525) + +class CurrentSense { + public: + CurrentSense(PinName current_pin); + + float get_current(void); + + private: + AnalogIn current_pin_; +}; + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.cpp Tue Nov 24 03:56:22 2015 +0000 @@ -0,0 +1,21 @@ +#include "Encoder.h" + +//Encoder steps to revolutions of output shaft in radians +float pulsesToRadians(float pulses) { + return ((pulses/ENC_STEPS_PER_REV)*(2.0*PI)); +} + +//Encoder steps to revolutions of output shaft in degrees +float pulsesToDegrees(float pulses) { + return ((pulses/ENC_STEPS_PER_REV)*360.0); +} + +Encoder::Encoder(PinName enc_a_pin, PinName enc_b_pin): qei_(enc_a_pin, enc_b_pin, NC, ENC_STEPS_PER_REV, QEI::X4_ENCODING) { + //Use X4 encoding. + //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2)) + //QEI ; +} + +float Encoder::get_position(void) { + return pulsesToRadians(qei_.getPulses()); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Encoder.h Tue Nov 24 03:56:22 2015 +0000 @@ -0,0 +1,22 @@ +#include "mbed.h" +#include "QEI.h" + +#ifndef ENCODER_H +#define ENCODER_H + + +#define PI 3.14159265358979323846 +#define ENC_STEPS_PER_REV 1200 + +class Encoder { + public: + Encoder(PinName enc_a_pin, PinName enc_b_pin); + + float get_position(void); + + private: + QEI qei_; + +}; + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HBridge.cpp Tue Nov 24 03:56:22 2015 +0000 @@ -0,0 +1,24 @@ +#include "HBridge.h" + +HBridge::HBridge(PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period): pwm_pin_(pwm_pin), min_1_pin_(min_1_pin), min_2_pin_(min_2_pin) { + pwm_pin_.period(pwm_period); +} + +//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 +void HBridge::set_output(float output) { + this->output = output; + if (output > 0) { + min_1_pin_.write(0); + min_2_pin_.write(1); + pwm_pin_.write(output); + } else if (output < 0) { + min_1_pin_.write(1); + min_2_pin_.write(0); + pwm_pin_.write(output * -1); + } else { + min_1_pin_.write(0); + min_2_pin_.write(0); + pwm_pin_.write(0.0); + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/HBridge.h Tue Nov 24 03:56:22 2015 +0000 @@ -0,0 +1,21 @@ +#include "mbed.h" + +#ifndef HBRIDGE_H +#define HBRIDGE_H + +class HBridge { + public: + HBridge(PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period); + + float output; + + void set_output(float output); + + private: + PwmOut pwm_pin_; + DigitalOut min_1_pin_; + DigitalOut min_2_pin_; + +}; + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.cpp Tue Nov 24 03:56:22 2015 +0000 @@ -0,0 +1,65 @@ +#include "Motor.h" + +Motor::Motor( + PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period, + PinName current_pin, + PinName enc_a_pin, PinName enc_b_pin, + float pid_cur_p_gain, float pid_cur_d_gain, float pid_cur_i_gain) : + hbridge_(pwm_pin, min_1_pin, min_2_pin, pwm_period), + current_sense_(current_pin), + encoder_(enc_a_pin, enc_b_pin), + current_controller_(pid_cur_p_gain, pid_cur_d_gain, pid_cur_i_gain) { + + timer_.start(); +} + +void Motor::update(void) { + this->velocity = this->get_velocity(); + this->current = this->get_current(); + current_controller_.set_command(this->command); + this->output = current_controller_.command_torque(this->current); + this->torque = current_controller_.current_torque; + this->hbridge_.set_output(this->output); +} + +float Motor::get_position(void) { + return encoder_.get_position(); +} + +// Implicitly updates position +float Motor::get_velocity(void) { + float old_position = this->position; + this->position = this->get_position(); + float velocity = (this->position - old_position)/timer_.read(); + timer_.reset(); + return velocity; +} + +float Motor::get_current(void) { + return current_sense_.get_current(); +} + +void Motor::set_command(float command) { + this->command = command; +} + +//Useful for testing peak torque. +//pulse_time = motor on time. pulse_interval = motor off time +/*float Motor::get_pulse_current(float pulse_time, float pulse_interval) { + timer.start(); + int count = 0; + float sum = 0.0; + this->write(1.0); + while (timer.read() < pulse_time){ + sum += current_pin_.read(); //Prints current draw to PC + count++; + //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); + this->write(0.0); + wait(pulse_interval); + return sum/count; +}*/ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Motor.h Tue Nov 24 03:56:22 2015 +0000 @@ -0,0 +1,50 @@ +#include "mbed.h" +#include "QEI.h" +#include "Encoder.h" +#include "HBridge.h" +#include "CurrentSense.h" +#include "PID.h" + +#ifndef MOTOR_H +#define MOTOR_H + +class Motor { + public: + Motor( + PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period, + PinName current_pin, + PinName enc_a_pin, PinName enc_b_pin, + float pid_cur_p_gain, float pid_cur_d_gain, float pid_cur_i_gain + ); + + float position; + float velocity; + float current; + float torque; + float command; + + // output to PWM + float output; + + // Update all state variables and apply command + void update(void); + + float get_position(void); + float get_velocity(void); + float get_current(void); + + // Set current controller commanded value + void set_command(float command); + + //float get_pulse_current(float pulse_time, float pulse_interval); + + private: + Timer timer_; + HBridge hbridge_; + CurrentSense current_sense_; + Encoder encoder_; + PIDController current_controller_; + +}; + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.cpp Tue Nov 24 03:56:22 2015 +0000 @@ -0,0 +1,91 @@ +#include "PID.h" + +PIDController::PIDController(float p_gain, float d_gain, float i_gain) { + kp = p_gain; + kd = d_gain; + ki = i_gain; + + command = 0.0; + error = 0.0; + old_error = 0.0; + integral_error = 0.0; + + current_torque = 0.0; + torque_command = 0.0; + torque_error = 0.0; + torque_integral_error = 0.0; + + direction = 0; +} + +void PIDController::set_command(float command) { + this->command = command; + this->torque_command = command; + this->integral_error = 0.0; + this->torque_integral_error = 0.0; +} + +//voltage mode position control +//This function is called to set the desired position of the servo +float PIDController::command_position(float current_position) { + this->error = (this->command - current_position); + this->integral_error += this->error; + if (this->integral_error > 1.0) + this->integral_error = 1.0; + else if (this->integral_error < -1.0) + this->integral_error = -1.0; + float output = (this->kp)*(this->error) + (this->kd)*(this->error - this->old_error) + (this->ki)*(this->integral_error); + this->old_error = this->error; + return output; +} + +// Torque Mode position control +float PIDController::command_position_tm(float current_position, float current_current) { + float output = this->command_position(current_position); + output += this->command_torque(current_current); + return output; +} + +// Given a current current in abs(Amps), produce a PWM command between -1.0 (full reverse) and 1.0 (full forward) +float PIDController::command_torque(float current_current) { + if (this->direction != 0){ + this->current_torque = this->direction*(current_current); + } else { + this->current_torque = current_current; + } + + float average_torque = 0; + + for (int i = 0; i < 4; i++){ + this->torque_history[i] = this->torque_history[i+1]; + average_torque += this->torque_history[i]; + } + average_torque += current_torque; + average_torque = average_torque/5; + //average_torque = current_torque; // Does this just overwrite the average? + this->torque_history[4] = current_torque; + + this->torque_error = (this->torque_command - average_torque); + + this->torque_integral_error += this->torque_error; + + float output = 4.0*this->torque_command + this->kp*this->torque_error + this->ki*this->torque_integral_error; + + // Set direction based on output + if (output > 0){ + this->direction = 1; + } else if(output < 0){ + this->direction = -1; + } else{ + output = 0; + } + + // Coerce ouptut to be between -1 and 1 + if (output > 1){ + output = 1; + } else if (output < -1){ + output = -1; + } + + return output; +} \ No newline at end of file
--- a/PID.h Fri Jul 12 00:13:37 2013 +0000 +++ b/PID.h Tue Nov 24 03:56:22 2015 +0000 @@ -2,53 +2,41 @@ //PID Controller class #include "mbed.h" -#ifndef PID_H -#define PID_H -class PIDController{ -public: - -PIDController(float desired_position, float desired_torque, float p_gainp, float d_gainp, float i_gain_p, float p_gain_c, float i_gain_c); -~PIDController(); - -float goal_position; -float current_position; - -float kp_p; -float kd_p; -float ki_p; - -float kp_c; -float ki_c; -float c_error; -float error_sum; -float command; -float torque_command; -float c_torque; +#ifndef PID_CONTROLLER_H +#define PID_CONTROLLER_H -float error; -float old_error; -float integral_error; - - -int counter; - -Timer timer; - -float torque; -float direction; +class PIDController { + public: + PIDController(float p_gain, float d_gain, float i_gain); + + float command_position(float current_position); + float command_torque(float current_current); + float command_position_tm(float current_position, float current_current); + + // Process variable commanded value + float command; + float current_torque; + + void set_command(float command); + + private: + float kp; + float kd; + float ki; + + // State for process variable (position for now) + float error; + float old_error; + float integral_error; + + // Current command (fuse with command?) + float torque_command; + float torque_error; + float torque_integral_error; + float torque_history[5]; + + float direction; +}; -float past_currents [5]; - -float command_position(void); -float command_torque(void); -float command_position_tm(void); -private: - - - - - - -}; #endif
--- a/QEI_lib.lib Fri Jul 12 00:13:37 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org \ No newline at end of file
--- 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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-src.lib Tue Nov 24 03:56:22 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-src/#a11c0372f0ba
--- a/mbed.bld Fri Jul 12 00:13:37 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da