Diff: main.cpp

Revision:

1:406519ff0f17

Parent:

0:5f4bc2d63814

--- a/main.cpp	Thu Oct 19 19:42:08 2017 +0000
+++ b/main.cpp	Thu Oct 26 10:54:02 2017 +0000
@@ -1,34 +1,45 @@
+#include "AnglePosition.h"
+#include "PIDControl.h"
 #include "mbed.h"
 #include "encoder.h"
 #include "Servo.h"
 #include "MODSERIAL.h"
-
-
-
-
+#include "FastPWM.h"
 
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
-MODSERIAL pc(USBTX, USBRX);
-Ticker MyControllerTicker1;
-Ticker MyControllerTicker2;
+MODSERIAL pc(USBTX, USBRX);         //
+Ticker MyControllerTicker1;         //Declare Ticker Motor 1
+Ticker MyControllerTicker2;         //Declare Ticker Motor 2
+AnglePosition Angle;                //Declare Angle calculater
+PIDControl PID;                     //Declare PID Controller
+AnalogIn potmeter1(A3);             //Set Inputpin
+AnalogIn potmeter2(A4);             //Set Inputpin
+
+const float CONTROLLER_TS = 0.02;   //Motor frequency
+
+//------------------------------------------------------------------------------
+//-------------------------Kinematic Constants----------------------------------
+//------------------------------------------------------------------------------
 const double RAD_PER_PULSE = 0.00074799825*2;
-const float CONTROLLER_TS = 0.5;
 const double PI = 3.14159265358979323846;
-//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
-
-
+const float max_rangex = 500.0;
+const float max_rangey = 300.0;
+const float x_offset = 156.15;
+const float y_offset = -76.97;
+const float alpha_offset = -(21.52/180)*PI;
+const float beta_offset  = 0.0;
+const float L1 = 450.0;
+const float L2 = 490.0;
 
 
 //------------------------------------------------------------------------------
 //--------------------------------Servo-----------------------------------------
 //------------------------------------------------------------------------------
-Servo MyServo(D9);
-InterruptIn But1(D8);
-int k=0;
+Servo MyServo(D9);                  //Declare button
+InterruptIn But1(D8);               //Declare used button
+int k=0;                            //Position flag
 
 void servo_control (){
     if (k==0){
@@ -38,144 +49,71 @@
     else{
         MyServo = 2;
         k=0;
-        }
-}
-//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
-
-
-
-
-
-//------------------------------------------------------------------------------
-//-------------------------------Kinematics-------------------------------------
-//------------------------------------------------------------------------------
-AnalogIn potmeter1(A3);
-AnalogIn potmeter2(A4);
-const float max_rangex = 300.0;
-const float max_rangey = 300.0;
-const float x_offset = 156.15;
-const float y_offset = -76.97;
-const float alpha_offset = -(21.52/180)*PI;
-const float beta_offset  = 0.0;
-const float L1 = 450.0;
-const float L2 = 490.0;
-
-float gettargetposition(double potmeter, int max_range){
-    float target = potmeter * max_range;
-    return target;
-}
-
-float getreferenceposition(float target, float offset) {
-    float  referenceposition = target + offset;
-    return referenceposition;
+    }
 }
 
-float getreferencealpha(float max_rangex, float maxrangey, float x_offset, float y_offset, float alpha_offset, float L1, float L2) {
-    float x_target = gettargetposition(potmeter1, max_rangex);
-    float y_target = gettargetposition(potmeter2, max_rangey);
-    float x = getreferenceposition(x_target, x_offset);
-    float y = getreferenceposition(y_target, y_offset);        
-    float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
-    float alpha = alpha_ + alpha_offset;
-    return alpha;
-}
-    
-float getreferencebeta(float max_rangex, float maxrangey, float x_offset, float y_offset, float beta_offset, float L1, float L2) {
-    float x_target = gettargetposition(potmeter1, max_rangex);
-    float y_target = gettargetposition(potmeter2, max_rangey);
-    float x = getreferenceposition(x_target, x_offset);
-    float y = getreferenceposition(y_target, y_offset);    
-    float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
-    float beta = acos( ((L1*L1) + (L2*L2) - (x*x) - (y*y)) / (2*L1*L2)) - alpha_ + beta_offset;
-    return beta;
-}
-//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
-
-
-
-
-
-//------------------------------------------------------------------------------
-//------------------------------PI_Controller-----------------------------------
-//------------------------------------------------------------------------------
-float PI_controller(float error, const float Kp, const float Ki, float Ts, double &e_int) {
-    e_int += Ts * error;
-    return Kp * error + Ki * e_int ;
-}
-//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
-
-
-
-
 
 //------------------------------------------------------------------------------
 //--------------------------------Motor1----------------------------------------
 //------------------------------------------------------------------------------
-PwmOut motor1(D5);
+FastPWM motor1(D5);
 DigitalOut motor1DirectionPin(D4);
 DigitalIn ENC2A(D12);
 DigitalIn ENC2B(D13);
 Encoder encoder1(D13,D12);
-const float MOTOR1_KP = 20.0;
-const float MOTOR1_KI = 10.0;
-double m1_err_int = 0;
+const float MOTOR1_KP = 40.0;
+const float MOTOR1_KI = 0.0;
+const float MOTOR1_KD = 15.0;
+double M1_v1 = 0.0;
+double M1_v2 = 0.0;
 const double motor1_gain = 2*PI;
+const float M1_N = 0.5;
 
 
 void motor1_control(){
-    float reference_alpha = getreferencebeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2);
+    float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, potmeter1, potmeter2);
     float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
     float error_alpha = reference_alpha-position_alpha;
-    float magnitude1 = PI_controller(error_alpha, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int)/ motor1_gain;
+    float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
     motor1 = fabs(magnitude1);
     pc.printf("err_a = %f  alpha = %f   mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
     pc.printf("\r\n");
-    
-    
-    if (magnitude1 < 0){
+    // Determine Motor Direction 
+    if (magnitude1  < 0){
         motor1DirectionPin = 1;
     }
     else{
         motor1DirectionPin = 0;
     }
 }
-//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
-//------------------------------------------------------------------------------
-
-
-
-
 
 //------------------------------------------------------------------------------
 //--------------------------------Motor2----------------------------------------
 //------------------------------------------------------------------------------
-PwmOut motor2(D6);
+FastPWM motor2(D6);
 DigitalOut motor2DirectionPin(D7);
 DigitalIn ENC1A(D10);
 DigitalIn ENC1B(D11);
 Encoder encoder2(D10,D11);
-const float MOTOR2_KP = 20.0;
-const float MOTOR2_KI = 10.0;
+const float MOTOR2_KP = 60.0;
+const float MOTOR2_KI = 0.0;
+const float MOTOR2_KD = 15.0;
 double m2_err_int = 0;
 const double motor2_gain = 2*PI;
+const float M2_N = 0.5;
+double M2_v1 = 0.0;
+double M2_v2 = 0.0;
 
 
 void motor2_control(){
-    float reference_beta = getreferencealpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2);
+    float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, potmeter1, potmeter2);
     float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
     float error_beta = reference_beta-position_beta;
-    float magnitude2 = PI_controller(error_beta, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int)/ motor2_gain;
+    float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
     motor2 = fabs(magnitude2);
     pc.printf("err2 = %f  beta = %f   mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
     pc.printf("\r\n");
-    
+    //Determine Motor Direction
     if (magnitude2 > 0){
         motor2DirectionPin = 1;
     }
@@ -187,21 +125,14 @@
 //------------------------------------------------------------------------------
 //------------------------------------------------------------------------------
 
-
-
-
-
 int main(){
     pc.baud(115200);
-    motor1.period(0.0002f);
-    motor2.period(0.0002f);
+    motor1.period(0.0001f);
+    motor2.period(0.0001f);
     MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
     MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
-    But1.rise(&servo_control);
-     
-    
-    
-    
-           
+    But1.rise(&servo_control);      
     while(1) {}   
-}
\ No newline at end of file
+}
+
+