Decoupled position and current control working.

Dependencies:   QEI mbed-src

Files at this revision

API Documentation at this revision

Comitter:
abuchan
Date:
Tue Nov 24 03:56:22 2015 +0000
Parent:
3:cae0b305d54c
Commit message:
Decoupled position and current control working.

Changed in this revision

CurrentSense.cpp Show annotated file Show diff for this revision Revisions of this file
CurrentSense.h Show annotated file Show diff for this revision Revisions of this file
Encoder.cpp Show annotated file Show diff for this revision Revisions of this file
Encoder.h Show annotated file Show diff for this revision Revisions of this file
HBridge.cpp Show annotated file Show diff for this revision Revisions of this file
HBridge.h Show annotated file Show diff for this revision Revisions of this file
Motor.cpp Show annotated file Show diff for this revision Revisions of this file
Motor.h Show annotated file Show diff for this revision Revisions of this file
PID.cpp Show annotated file Show diff for this revision Revisions of this file
PID.h Show annotated file Show diff for this revision Revisions of this file
QEI_lib.lib Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-src.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CurrentSense.cpp	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,7 @@
+#include "CurrentSense.h"
+
+CurrentSense::CurrentSense(PinName current_pin): current_pin_(current_pin) {}
+
+float CurrentSense::get_current(void) {
+    return ADC_TO_CURRENT_SCALE * current_pin_.read();
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CurrentSense.h	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,19 @@
+#include "mbed.h"
+
+#ifndef CURRENT_SENSE_H
+#define CURRENT_SENSE_H
+
+// 3.3V max ADC reading / 0.525V/A from the current amp
+#define ADC_TO_CURRENT_SCALE (3.3/.525)
+
+class CurrentSense {
+    public:
+        CurrentSense(PinName current_pin);
+        
+        float get_current(void);
+    
+    private:
+        AnalogIn current_pin_;
+};
+
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.cpp	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,21 @@
+#include "Encoder.h"
+
+//Encoder steps to revolutions of output shaft in radians
+float pulsesToRadians(float pulses) {
+    return ((pulses/ENC_STEPS_PER_REV)*(2.0*PI));
+}
+
+//Encoder steps to revolutions of output shaft in degrees
+float pulsesToDegrees(float pulses) {
+    return ((pulses/ENC_STEPS_PER_REV)*360.0);
+}
+
+Encoder::Encoder(PinName enc_a_pin, PinName enc_b_pin): qei_(enc_a_pin, enc_b_pin, NC, ENC_STEPS_PER_REV, QEI::X4_ENCODING) {
+    //Use X4 encoding.
+    //(encoder channel 1, encoder channel 2, index (n/a here), counts per revolution, mode (X4, X2))
+    //QEI ;
+}
+
+float Encoder::get_position(void) {
+    return pulsesToRadians(qei_.getPulses());
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.h	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,22 @@
+#include "mbed.h"
+#include "QEI.h"
+
+#ifndef ENCODER_H
+#define ENCODER_H
+
+
+#define PI  3.14159265358979323846
+#define ENC_STEPS_PER_REV 1200
+
+class Encoder {
+    public:
+        Encoder(PinName enc_a_pin, PinName enc_b_pin);
+        
+        float get_position(void);
+        
+    private:
+        QEI qei_;
+
+};
+
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HBridge.cpp	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,24 @@
+#include "HBridge.h"
+
+HBridge::HBridge(PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period): pwm_pin_(pwm_pin), min_1_pin_(min_1_pin), min_2_pin_(min_2_pin) {
+    pwm_pin_.period(pwm_period);
+}
+
+//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
+void HBridge::set_output(float output) {
+    this->output = output;
+    if (output > 0) {
+        min_1_pin_.write(0);
+        min_2_pin_.write(1);
+        pwm_pin_.write(output);
+    } else if (output < 0) {
+        min_1_pin_.write(1);
+        min_2_pin_.write(0);
+        pwm_pin_.write(output * -1);
+    } else {
+        min_1_pin_.write(0);
+        min_2_pin_.write(0);
+        pwm_pin_.write(0.0);
+    }
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HBridge.h	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,21 @@
+#include "mbed.h"
+
+#ifndef HBRIDGE_H
+#define HBRIDGE_H
+
+class HBridge {
+    public:
+        HBridge(PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period);
+        
+        float output;
+        
+        void set_output(float output);
+        
+    private:
+        PwmOut pwm_pin_;
+        DigitalOut min_1_pin_;
+        DigitalOut min_2_pin_;
+
+};
+
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.cpp	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,65 @@
+#include "Motor.h"
+
+Motor::Motor(
+    PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period,
+    PinName current_pin,
+    PinName enc_a_pin, PinName enc_b_pin, 
+    float pid_cur_p_gain, float pid_cur_d_gain, float pid_cur_i_gain) : 
+    hbridge_(pwm_pin, min_1_pin, min_2_pin, pwm_period), 
+    current_sense_(current_pin), 
+    encoder_(enc_a_pin, enc_b_pin), 
+    current_controller_(pid_cur_p_gain, pid_cur_d_gain, pid_cur_i_gain) {
+    
+    timer_.start();
+}
+
+void Motor::update(void) {
+    this->velocity = this->get_velocity();
+    this->current = this->get_current();
+    current_controller_.set_command(this->command);
+    this->output = current_controller_.command_torque(this->current);
+    this->torque = current_controller_.current_torque;
+    this->hbridge_.set_output(this->output);
+}
+
+float Motor::get_position(void) {
+    return encoder_.get_position();
+}
+
+// Implicitly updates position
+float Motor::get_velocity(void) {
+    float old_position = this->position;
+    this->position = this->get_position();
+    float velocity = (this->position - old_position)/timer_.read();
+    timer_.reset();
+    return velocity;
+}
+
+float Motor::get_current(void) {
+    return current_sense_.get_current();
+}
+
+void Motor::set_command(float command) {
+    this->command = command;
+}    
+
+//Useful for testing peak torque.
+//pulse_time = motor on time.  pulse_interval = motor off time
+/*float Motor::get_pulse_current(float pulse_time, float pulse_interval) {   
+    timer.start();
+    int count = 0;
+    float sum = 0.0;
+    this->write(1.0);
+    while (timer.read() < pulse_time){
+        sum += current_pin_.read();                    //Prints current draw to PC
+        count++;
+        //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);
+    this->write(0.0);
+    wait(pulse_interval);
+    return sum/count;
+}*/
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.h	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,50 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "Encoder.h"
+#include "HBridge.h"
+#include "CurrentSense.h"
+#include "PID.h"
+
+#ifndef MOTOR_H
+#define MOTOR_H
+
+class Motor {
+    public:
+        Motor(
+            PinName pwm_pin, PinName min_1_pin, PinName min_2_pin, float pwm_period,
+            PinName current_pin,
+            PinName enc_a_pin, PinName enc_b_pin, 
+            float pid_cur_p_gain, float pid_cur_d_gain, float pid_cur_i_gain
+        );
+        
+        float position;
+        float velocity;
+        float current;
+        float torque;
+        float command;
+        
+        // output to PWM
+        float output;
+        
+        // Update all state variables and apply command
+        void update(void);
+        
+        float get_position(void);
+        float get_velocity(void);
+        float get_current(void);
+        
+        // Set current controller commanded value
+        void set_command(float command);
+        
+        //float get_pulse_current(float pulse_time, float pulse_interval);
+    
+    private:
+        Timer timer_;
+        HBridge hbridge_;
+        CurrentSense current_sense_;
+        Encoder encoder_;
+        PIDController current_controller_;
+        
+};
+        
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.cpp	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,91 @@
+#include "PID.h"
+
+PIDController::PIDController(float p_gain, float d_gain, float i_gain) {
+    kp = p_gain;
+    kd = d_gain;
+    ki = i_gain;
+
+    command = 0.0;
+    error = 0.0;
+    old_error = 0.0;
+    integral_error = 0.0;
+    
+    current_torque = 0.0;
+    torque_command = 0.0;
+    torque_error = 0.0;
+    torque_integral_error = 0.0;
+    
+    direction = 0;
+}
+
+void PIDController::set_command(float command) {
+    this->command = command;
+    this->torque_command = command;
+    this->integral_error = 0.0;
+    this->torque_integral_error = 0.0;
+}
+
+//voltage mode position control
+//This function is called to set the desired position of the servo
+float PIDController::command_position(float current_position) {
+    this->error = (this->command - current_position);
+    this->integral_error += this->error;
+    if (this->integral_error > 1.0)
+        this->integral_error = 1.0;
+    else if (this->integral_error < -1.0)
+        this->integral_error = -1.0;
+    float output = (this->kp)*(this->error) + (this->kd)*(this->error - this->old_error) + (this->ki)*(this->integral_error);
+    this->old_error = this->error;
+    return output;
+}
+
+// Torque Mode position control
+float PIDController::command_position_tm(float current_position, float current_current) {
+    float output = this->command_position(current_position);
+    output += this->command_torque(current_current);
+    return output;
+}
+
+// Given a current current in abs(Amps), produce a PWM command between -1.0 (full reverse) and 1.0 (full forward)
+float PIDController::command_torque(float current_current) {
+    if (this->direction != 0){
+        this->current_torque = this->direction*(current_current);
+    } else {
+        this->current_torque = current_current;
+    }
+    
+    float average_torque = 0;
+   
+    for (int i = 0; i < 4; i++){
+        this->torque_history[i] = this->torque_history[i+1];
+        average_torque += this->torque_history[i];
+    }
+    average_torque += current_torque;
+    average_torque = average_torque/5;
+    //average_torque = current_torque; // Does this just overwrite the average?
+    this->torque_history[4] = current_torque;
+    
+    this->torque_error = (this->torque_command - average_torque);
+
+    this->torque_integral_error += this->torque_error;
+    
+    float output = 4.0*this->torque_command + this->kp*this->torque_error + this->ki*this->torque_integral_error;
+    
+    // Set direction based on output
+    if (output > 0){
+        this->direction = 1;
+    } else if(output < 0){
+        this->direction = -1;
+    } else{
+        output = 0;
+    }
+   
+    // Coerce ouptut to be between -1 and 1
+    if (output > 1){
+        output = 1;
+    } else if (output < -1){
+        output = -1;
+    }
+    
+    return output;
+}
\ No newline at end of file
--- a/PID.h	Fri Jul 12 00:13:37 2013 +0000
+++ b/PID.h	Tue Nov 24 03:56:22 2015 +0000
@@ -2,53 +2,41 @@
 //PID Controller class
 
 #include "mbed.h"
-#ifndef PID_H
-#define PID_H
 
-class PIDController{
-public:
-
-PIDController(float desired_position, float desired_torque, float p_gainp, float d_gainp, float i_gain_p, float p_gain_c, float i_gain_c);
-~PIDController();
-
-float goal_position;
-float current_position;
-
-float kp_p;
-float kd_p;
-float ki_p;
-
-float kp_c;
-float ki_c;
-float c_error;
-float error_sum;
-float command;
-float torque_command;
-float c_torque;
+#ifndef PID_CONTROLLER_H
+#define PID_CONTROLLER_H
 
-float error;
-float old_error;
-float integral_error;
-
-
-int counter;
-
-Timer timer;
-
-float torque;
-float direction;
+class PIDController {
+    public:
+        PIDController(float p_gain, float d_gain, float i_gain);
+    
+        float command_position(float current_position);
+        float command_torque(float current_current);
+        float command_position_tm(float current_position, float current_current);
+        
+        // Process variable commanded value
+        float command;
+        float current_torque;
+        
+        void set_command(float command);
+    
+    private:
+        float kp;
+        float kd;
+        float ki;
+        
+        // State for process variable (position for now)
+        float error;
+        float old_error;
+        float integral_error;
+        
+        // Current command (fuse with command?)
+        float torque_command;
+        float torque_error;
+        float torque_integral_error;
+        float torque_history[5];
+        
+        float direction;
+};
 
-float past_currents [5];
-
-float command_position(void);
-float command_torque(void);
-float command_position_tm(void);
-private:
-
-
-
-
-
-
-};
 #endif 
--- a/QEI_lib.lib	Fri Jul 12 00:13:37 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org
\ No newline at end of file
--- a/main.cpp	Fri Jul 12 00:13:37 2013 +0000
+++ b/main.cpp	Tue Nov 24 03:56:22 2015 +0000
@@ -1,227 +1,104 @@
 //Ben Katz, 2013
-
-#include "QEI.h"
+//Austin Buchan, 2015
+#include "mbed.h"
+#include "Motor.h"
 #include "PID.h"
 
-#define pi  3.14159265358979323846
-
-
-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);
-
-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;
-
-
+#define PWM_PIN PTB13
+#define MIN_1_PIN PTA0
+#define MIN_2_PIN PTA8
+#define ENC_A_PIN PTB6
+#define ENC_B_PIN PTB7
+#define CURRENT_PIN PTA9
 
-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);
-}
-
+#define UART_RX_PIN PTB1
+#define UART_TX_PIN PTB2
 
-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);
-    }
-}
+#define PWM_PERIOD 0.001
+#define PID_CUR_PERIOD 0.001
+#define PID_POS_PERIOD 0.005
+
+#define PID_POS_P_GAIN  0.5
+#define PID_POS_D_GAIN  10.0
+#define PID_POS_I_GAIN  0.1
 
-
-
-void pulse_motor(float pulse_time, float pulse_interval)  //Usefull for testing peak torque.
-                                                          //pulse_time = motor on time.  pulse_interval = motor off time
-{   
+#define PID_CUR_P_GAIN  0.5
+#define PID_CUR_D_GAIN  0.0
+//#define PID_CUR_I_GAIN  0.001
+#define PID_CUR_I_GAIN  0.01
 
-    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);
-}
-
+Serial pc(UART_TX_PIN, UART_RX_PIN);
 
-
-int main()
-{
+Motor motor(
+    PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD, 
+    CURRENT_PIN,
+    ENC_A_PIN, ENC_B_PIN, 
+    PID_CUR_P_GAIN, PID_CUR_D_GAIN, PID_CUR_I_GAIN
+);
 
-    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)
-    //timer.start();
-    //float currentSum = 0;
-    //float angleSum = 0;
-    
-    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()));
-    }
-    
- 
-    
+//HBridge hbridge(PWM_PIN, MIN_1_PIN, MIN_2_PIN, PWM_PERIOD);
+//CurrentSense current_sense(CURRENT_PIN);
+//Encoder encoder(ENC_A_PIN, ENC_B_PIN);
+
+PIDController position_controller(
+    PID_POS_P_GAIN, PID_POS_D_GAIN, PID_POS_I_GAIN
+);
+
+Ticker current_ticker;
+
+void update_current(void) {
+    motor.update();
 }
 
-
-
-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;
-    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;
-}
+Ticker position_ticker;
 
-//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;
-
+void update_position(void) {
+    motor.set_command(position_controller.command_position(motor.get_position()));
 }
 
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
 
-float PIDController::command_position_tm(void){  //Torque Mode position control
-    //wait(.0003);    
-    ///*
-   
+int main() {
+    printf("Built " __DATE__ " " __TIME__ "\r\n");
+    
+    led1 = 1;
+    led2 = 1;
+    led3 = 1;
+    
+    wait(.5);
+    
+    led2 = 0;
+    printf("Initializing\n\r");
+    
+    current_ticker.attach(&update_current, PID_CUR_PERIOD);
+    position_ticker.attach(&update_position, PID_POS_PERIOD);
+    
+    int count = 0;
+    float command = 0.0;
     
-   // */
-    //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();     
-       }  
+    led3 = 0;
+    printf("Starting\n\r");
     
-
-    
-
-    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);
+    while(1){
+        //command = 0.05 * abs((50-count)/10) - 0.1;
+        //motor.set_command(command);
+        
+        command = 1.0 * abs((55-count)/10) - 0.25;
+        position_controller.set_command(command);
+        
+        led1 = (count % 2);
+        
+        //printf("C:%F Cu:%F P:%F\n\r", command, current_sense.get_current(), encoder.get_position());
+        //printf("PC:%F\n\r", position_controller.command);
+        printf("MP:%F MV:%F MCu:%F MCo:%F MO:%F MT:%F P:%F\n\r", 
+            motor.position, motor.velocity, motor.current, motor.command, motor.output, motor.torque, position_controller.command
+        );
+        
+        
+        count = (count + 1) % 100;
+        
+        wait(0.1);
     }
-    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;
-}
+}
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-src.lib	Tue Nov 24 03:56:22 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-src/#a11c0372f0ba
--- a/mbed.bld	Fri Jul 12 00:13:37 2013 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da