Diff: main.cpp

Revision:

4:4e7b392ed0aa

Parent:

3:cae0b305d54c

Child:

5:75b5c24e36ad

--- a/main.cpp	Fri Jul 12 00:13:37 2013 +0000
+++ b/main.cpp	Wed Mar 18 17:17:58 2015 +0000
@@ -8,9 +8,6 @@
 
 Serial pc(USBTX, USBRX);
 
-//Use X4 encoding.
-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);
@@ -23,6 +20,13 @@
 Timer timer;
 
 
+// Setup parameters
+float volt2amp = 3.3 / .525; //(0-3.3V input) * (voltage divider muiltiplier) / (.525 V/A)
+float PWM_frequency = 20000.0;  // MC33926 h-bridge limited 20 kHz (.00005 seconds)
+
+//Use X4 encoding.
+QEI wheel(p13, p14, NC, steps_per_rev, QEI::X4_ENCODING);       //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2))
+
 
 float pulsesToRadians(float pulses) //Encoder steps to revolutions of output shaft in radians
 {
@@ -54,53 +58,61 @@
 
 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();
 
-    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)
-    
+      Motor1.write(1);
+      Motor2.write(0);
+      //Prints current draw to PC
+      printf("%F", (Current.read())*volt2amp);  //Arithmetic converts the analog input to Amps
+      printf("\n");
     }
+
     timer.stop();
     timer.reset();
+
     wait(pulse_time);
+
     Motor1.write(0);
     Motor2.write(0);
+
     wait(pulse_interval);
-}
+  }
 
 
 
 int main()
 {
 
-    Motor1.period(.00005);       //Set PWM frequency.  MC33926 h-bridge limited 20 kHz (.00005 seconds)
-    Motor2.period(.00005);
-    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)
+    Motor1.period(1 / PWM_frequency);       //Set PWM frequency.  MC33926 h-bridge limited 20 kHz (.00005 seconds)
+    Motor2.period(1 / PWM_frequency);
+
+    float desired_position = 40;
+    float torque = 0.1;
+    float position_P_gain = 0.1;
+    float position_D_gain = 3;
+    float position_I_gain = 0;
+    float torque_P_gain = 0.0007;
+    float torque_I_gain = 0.0000001;
+
+
+    PIDController controller(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;
-    
-    wait(.7);
-   
-    wait(.5);
+
+    wait(1.5);
+
     printf("%F", pulsesToDegrees(wheel.getPulses()));
     printf("\n\r");
+
     while(1){
         signal_to_hbridge(controller.command_position_tm());
-        //printf("%F", pulsesToDegrees(wheel.getPulses()));
+        printf("%F", pulsesToDegrees(wheel.getPulses()));
     }
-    
- 
-    
+
 }
 
 
-
 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 = p_gain_p;
@@ -108,16 +120,19 @@
     ki_p = i_gain_p;
     kp_c = p_gain_c;
     ki_c = i_gain_c;
+
     torque = desired_torque;
+    goal_position = desired_position;
+
     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;
 }
@@ -125,10 +140,10 @@
 //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.  
+    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;
@@ -150,48 +165,43 @@
 
 
 float PIDController::command_position_tm(void){  //Torque Mode position control
-    //wait(.0003);    
-    ///*
-   
-    
-   // */
-    //this->current_position = pulsesToDegrees(wheel.getPulses());  //Change pulsesToDegrees() to pulsesToRadians() if you would prefer to use radians
-    float desired_position = this->goal_position;  
+    wait(.0003);
+
+    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();     
-       }  
-    
+        this->torque_command = command;
+        this->timer.reset();
+       }
 
-    
 
     return this->command_torque();
-    
+
 
 }
 
 
 
 float PIDController::command_torque(void){
-    //wait(.0004);
+    wait(.0004);
     float current_torque;
-    
+
     if (this->direction != 0){
-        current_torque = this->direction*(Current.read()*3.3/.525);
+        current_torque = this->direction*(Current.read() * volt2amp);
     }
     else{
-        current_torque = (Current.read()*3.3/.525);
+        current_torque = (Current.read() * volt2amp);
     }
    float average = 0;
-   
+
     for (int i = 0; i < 4; i++){
         this->past_currents[i] = this->past_currents[i+1];
         average += past_currents[i];
@@ -200,12 +210,12 @@
     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;
@@ -222,6 +232,6 @@
    else if (this->torque_command < -1){
         this->torque_command = -1;
         }
-    
+
     return this->torque_command;
 }