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;
+
+}