Diff: main.cpp

Revision:

3:cae0b305d54c

Parent:

2:89bb6272869b

Child:

4:5ae9f8b3a16f

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