jiexuan fan / ServoController_2

mit

Dependencies:   QEI mbed-src

main.cpp@2:89bb6272869b, 2013-06-28 (annotated)

Committer:
benkatz
Date:
Fri Jun 28 19:03:06 2013 +0000
Revision:
2:89bb6272869b
Parent:
1:30696e4d196b
Child:
3:cae0b305d54c
V1.0;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 2:89bb6272869b 1 //Ben Katz, 2013
benkatz 2:89bb6272869b 2
aberk 0:bcff39fac858 3 #include "QEI.h"
benkatz 2:89bb6272869b 4 #include "PID.h"
benkatz 2:89bb6272869b 5
benkatz 2:89bb6272869b 6 #define pi 3.14159265358979323846
benkatz 2:89bb6272869b 7
aberk 0:bcff39fac858 8
aberk 0:bcff39fac858 9 Serial pc(USBTX, USBRX);
aberk 1:30696e4d196b 10 //Use X4 encoding.
benkatz 2:89bb6272869b 11 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))
benkatz 2:89bb6272869b 12
benkatz 2:89bb6272869b 13
benkatz 2:89bb6272869b 14 DigitalOut EncoderVcc (p18); //Encoder VCC pin. Alternatively, just connect these to the mbed's vout and gnd, but tihs can make wiring easier
benkatz 2:89bb6272869b 15 DigitalOut EncoderGnd(p19); //Encoder ground pin
benkatz 2:89bb6272869b 16 PwmOut Motor1(p21); //Ouput pin to h-bridge 1
benkatz 2:89bb6272869b 17 PwmOut Motor2 (p22); //Output pin to h-bridge 2
benkatz 2:89bb6272869b 18 AnalogIn Current (p20); //Current sense pin
benkatz 2:89bb6272869b 19
benkatz 2:89bb6272869b 20 int steps_per_rev = 1200; //Encoder CPR * gearbox ratio
benkatz 2:89bb6272869b 21 Timer timer;
benkatz 2:89bb6272869b 22
benkatz 2:89bb6272869b 23
benkatz 2:89bb6272869b 24
benkatz 2:89bb6272869b 25 float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
benkatz 2:89bb6272869b 26 {
benkatz 2:89bb6272869b 27 return ((pulses/steps_per_rev)*(2*pi));
benkatz 2:89bb6272869b 28 }
benkatz 2:89bb6272869b 29
benkatz 2:89bb6272869b 30 float pulsesToDegrees(float pulses) //Encoder steps to revolutions of output shaft in degrees
benkatz 2:89bb6272869b 31 {
benkatz 2:89bb6272869b 32 return ((pulses/steps_per_rev)*360);
benkatz 2:89bb6272869b 33 }
benkatz 2:89bb6272869b 34
benkatz 2:89bb6272869b 35
benkatz 2:89bb6272869b 36 void signal_to_hbridge( float signal) //Input of -1 means full reverse, 1 means full forward
benkatz 2:89bb6272869b 37 { //An input of magnitude > 1 gets reduced to 1 (or -1) by the pwm.write() function
benkatz 2:89bb6272869b 38 if (signal < 0) {
benkatz 2:89bb6272869b 39 Motor1.write(0);
benkatz 2:89bb6272869b 40 Motor2.write(signal * -1);
benkatz 2:89bb6272869b 41 } else if (signal > 0) {
benkatz 2:89bb6272869b 42 Motor1.write(signal);
benkatz 2:89bb6272869b 43 Motor2.write(0);
benkatz 2:89bb6272869b 44 } else {
benkatz 2:89bb6272869b 45 Motor1.write(0);
benkatz 2:89bb6272869b 46 Motor2.write(0);
benkatz 2:89bb6272869b 47 }
benkatz 2:89bb6272869b 48 }
aberk 0:bcff39fac858 49
benkatz 2:89bb6272869b 50 void pulse_motor(float pulse_time, float pulse_interval) //Usefull for testing peak torque.
benkatz 2:89bb6272869b 51 //pulse_time = motor on time. pulse_interval = motor off time
benkatz 2:89bb6272869b 52 {
benkatz 2:89bb6272869b 53 timer.start();
benkatz 2:89bb6272869b 54 while (timer.read() < pulse_time){
benkatz 2:89bb6272869b 55 Motor1.write(1);
benkatz 2:89bb6272869b 56 Motor2.write(0);
benkatz 2:89bb6272869b 57 //Prints current draw to PC
benkatz 2:89bb6272869b 58 printf("%F", (Current.read())*3.3*4/.525); //Arithmetic converts the analog input to Amps
benkatz 2:89bb6272869b 59 printf("\n"); //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A)
benkatz 2:89bb6272869b 60
benkatz 2:89bb6272869b 61 }
benkatz 2:89bb6272869b 62 timer.stop();
benkatz 2:89bb6272869b 63 timer.reset();
benkatz 2:89bb6272869b 64 wait(pulse_time);
benkatz 2:89bb6272869b 65 Motor1.write(0);
benkatz 2:89bb6272869b 66 Motor2.write(0);
benkatz 2:89bb6272869b 67 wait(pulse_interval);
benkatz 2:89bb6272869b 68 }
benkatz 2:89bb6272869b 69
benkatz 2:89bb6272869b 70
aberk 0:bcff39fac858 71
benkatz 2:89bb6272869b 72 int main()
benkatz 2:89bb6272869b 73 {
benkatz 2:89bb6272869b 74 EncoderVcc.write(1); //Encoder VCC and GND attached to digital output pins to make wiring easier
benkatz 2:89bb6272869b 75 EncoderGnd.write(0);
benkatz 2:89bb6272869b 76 Motor1.period(.00005); //Set PWM frequency. MC33926 h-bridge limited 20 kHz (.00005 seconds)
benkatz 2:89bb6272869b 77 Motor2.period(.00005);
benkatz 2:89bb6272869b 78 PIDController controller(180, 0.05, 3, 0); //(desired position, P gain, D gain, I gain)
benkatz 2:89bb6272869b 79
benkatz 2:89bb6272869b 80 /*
benkatz 2:89bb6272869b 81 Insert servo control loop below
benkatz 2:89bb6272869b 82 */
benkatz 2:89bb6272869b 83
benkatz 2:89bb6272869b 84
benkatz 2:89bb6272869b 85 while(1) {
benkatz 2:89bb6272869b 86
benkatz 2:89bb6272869b 87 signal_to_hbridge(controller.Update());
benkatz 2:89bb6272869b 88
aberk 0:bcff39fac858 89 }
benkatz 2:89bb6272869b 90 }
benkatz 2:89bb6272869b 91
benkatz 2:89bb6272869b 92
benkatz 2:89bb6272869b 93
benkatz 2:89bb6272869b 94 PIDController::PIDController(float desired_position, float p_gain, float d_gain, float i_gain)
benkatz 2:89bb6272869b 95 {
benkatz 2:89bb6272869b 96 kp = p_gain;
benkatz 2:89bb6272869b 97 kd = d_gain;
benkatz 2:89bb6272869b 98 ki = i_gain;
benkatz 2:89bb6272869b 99 error = 0;
benkatz 2:89bb6272869b 100 old_error = 0;
benkatz 2:89bb6272869b 101 goal_position = desired_position;
aberk 0:bcff39fac858 102
aberk 0:bcff39fac858 103 }
benkatz 2:89bb6272869b 104
benkatz 2:89bb6272869b 105
benkatz 2:89bb6272869b 106 float PIDController::Update(void) //This function is called to set the desired position of the servo
benkatz 2:89bb6272869b 107 {
benkatz 2:89bb6272869b 108 wait(.0004); //This delay allows enough time to register a position difference between cycles.
benkatz 2:89bb6272869b 109 //Without the delay, velocity is read as 0, so there is no D feedback.
benkatz 2:89bb6272869b 110 //The delay can be decreased when adding more servos, as the computation time becomes more significant.
benkatz 2:89bb6272869b 111 float desired_position = this->goal_position; //All value are stored in the PIDController object created, and can be changed at any time
benkatz 2:89bb6272869b 112 float p_gain = this->kp;
benkatz 2:89bb6272869b 113 float d_gain = this->kd;
benkatz 2:89bb6272869b 114 float i_gain = this->ki;
benkatz 2:89bb6272869b 115
benkatz 2:89bb6272869b 116 float currentPosition = pulsesToDegrees(wheel.getPulses()); //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
benkatz 2:89bb6272869b 117
benkatz 2:89bb6272869b 118 this->error = (currentPosition - desired_position);
benkatz 2:89bb6272869b 119 this->integral_error += this->error;
benkatz 2:89bb6272869b 120
benkatz 2:89bb6272869b 121 float command = p_gain*(this->error) + d_gain*(this->error - this->old_error) + i_gain*(this->integral_error);
benkatz 2:89bb6272869b 122
benkatz 2:89bb6272869b 123 this->old_error = error;
benkatz 2:89bb6272869b 124
benkatz 2:89bb6272869b 125 return command;
benkatz 2:89bb6272869b 126
benkatz 2:89bb6272869b 127 }