Peter Knoben / Mbed 2 deprecated Robot_control

Working EMG signals Mode control Direction control Position control Speed control

Dependencies:   AnglePosition2 Encoder FastPWM HIDScope MODSERIAL Movement PIDController Servo SignalNumber2 biquadFilter mbed

Fork of kinematics_control_copyfds by Peter Knoben

Files at this revision

API Documentation at this revision

Revision 1:406519ff0f17, committed 2017-10-26

Comitter:
peterknoben
Date:
Thu Oct 26 10:54:02 2017 +0000
Parent:
0:5f4bc2d63814
Child:
2:2ad485d762f5
Commit message:
Inclusief header

Changed in this revision

AnglePosition.lib Show annotated file Show diff for this revision Revisions of this file
FastPWM.lib Show annotated file Show diff for this revision Revisions of this file
PIDController.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AnglePosition.lib	Thu Oct 26 10:54:02 2017 +0000
@@ -0,0 +1,1 @@
+AnglePosition#d7e19af20f93

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FastPWM.lib	Thu Oct 26 10:54:02 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/Sissors/code/FastPWM/#c0b2265cff9c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PIDController.lib	Thu Oct 26 10:54:02 2017 +0000
@@ -0,0 +1,1 @@
+PIDController#b076b5e36978

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