![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mit
Diff: main.cpp
- Revision:
- 2:89bb6272869b
- Parent:
- 1:30696e4d196b
- Child:
- 3:cae0b305d54c
--- a/main.cpp Wed Aug 11 09:15:10 2010 +0000 +++ b/main.cpp Fri Jun 28 19:03:06 2013 +0000 @@ -1,16 +1,127 @@ +//Ben Katz, 2013 + #include "QEI.h" +#include "PID.h" + +#define pi 3.14159265358979323846 + Serial pc(USBTX, USBRX); //Use X4 encoding. -//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING); -//Use X2 encoding by default. -QEI wheel (p29, p30, NC, 624); +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); + } +} -int main() { +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); +} + + - while(1){ - wait(0.1); - pc.printf("Pulses is: %i\n", wheel.getPulses()); +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; + +}