![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mit
main.cpp
- Committer:
- benkatz
- Date:
- 2013-06-28
- Revision:
- 2:89bb6272869b
- Parent:
- 1:30696e4d196b
- Child:
- 3:cae0b305d54c
File content as of revision 2:89bb6272869b:
//Ben Katz, 2013 #include "QEI.h" #include "PID.h" #define pi 3.14159265358979323846 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)) 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 int steps_per_rev = 1200; //Encoder CPR * gearbox ratio Timer timer; 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); } 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); } } 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); 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); } 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 */ while(1) { signal_to_hbridge(controller.Update()); } } PIDController::PIDController(float desired_position, float p_gain, float d_gain, float i_gain) { kp = p_gain; kd = d_gain; ki = i_gain; error = 0; old_error = 0; goal_position = desired_position; } 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 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; float command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error); this->old_error = error; return command; }