![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mit
Diff: main.cpp
- Revision:
- 3:cae0b305d54c
- Parent:
- 2:89bb6272869b
- Child:
- 4:4e7b392ed0aa
diff -r 89bb6272869b -r cae0b305d54c main.cpp --- a/main.cpp Fri Jun 28 19:03:06 2013 +0000 +++ b/main.cpp Fri Jul 12 00:13:37 2013 +0000 @@ -7,12 +7,14 @@ Serial pc(USBTX, USBRX); + //Use X4 encoding. -QEI wheel(p16, p17, NC, 1200, QEI::X4_ENCODING); //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2)) +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); -DigitalOut EncoderVcc (p18); //Encoder VCC pin. Alternatively, just connect these to the mbed's vout and gnd, but tihs can make wiring easier -DigitalOut EncoderGnd(p19); //Encoder ground pin PwmOut Motor1(p21); //Ouput pin to h-bridge 1 PwmOut Motor2 (p22); //Output pin to h-bridge 2 AnalogIn Current (p20); //Current sense pin @@ -29,6 +31,7 @@ float pulsesToDegrees(float pulses) //Encoder steps to revolutions of output shaft in degrees { + return ((pulses/steps_per_rev)*360); } @@ -47,9 +50,12 @@ } } + + void pulse_motor(float pulse_time, float pulse_interval) //Usefull for testing peak torque. //pulse_time = motor on time. pulse_interval = motor off time -{ +{ + timer.start(); while (timer.read() < pulse_time){ Motor1.write(1); @@ -71,57 +77,151 @@ int main() { - EncoderVcc.write(1); //Encoder VCC and GND attached to digital output pins to make wiring easier - EncoderGnd.write(0); + Motor1.period(.00005); //Set PWM frequency. MC33926 h-bridge limited 20 kHz (.00005 seconds) Motor2.period(.00005); - PIDController controller(180, 0.05, 3, 0); //(desired position, P gain, D gain, I gain) - - /* - Insert servo control loop below - */ + 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; - - while(1) { + 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())); + } - signal_to_hbridge(controller.Update()); - - } + + } -PIDController::PIDController(float desired_position, float p_gain, float d_gain, float i_gain) +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_gain; - kd = d_gain; - ki = i_gain; + 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; +} + +//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; } -float PIDController::Update(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; - float d_gain = this->kd; - float i_gain = this->ki; +float PIDController::command_position_tm(void){ //Torque Mode position control + //wait(.0003); + ///* + - 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->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(); + } + - float command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error); + - this->old_error = error; - - return command; + 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); + } + 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; +}