Diff: main.cpp

Revision:

0:5f4bc2d63814

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 19 19:42:08 2017 +0000
@@ -0,0 +1,207 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "Servo.h"
+#include "MODSERIAL.h"
+
+
+
+
+
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+MODSERIAL pc(USBTX, USBRX);
+Ticker MyControllerTicker1;
+Ticker MyControllerTicker2;
+const double RAD_PER_PULSE = 0.00074799825*2;
+const float CONTROLLER_TS = 0.5;
+const double PI = 3.14159265358979323846;
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+
+
+
+
+//------------------------------------------------------------------------------
+//--------------------------------Servo-----------------------------------------
+//------------------------------------------------------------------------------
+Servo MyServo(D9);
+InterruptIn But1(D8);
+int k=0;
+
+void servo_control (){
+    if (k==0){
+        MyServo = 0;
+        k=1;
+    }
+    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);
+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 double motor1_gain = 2*PI;
+
+
+void motor1_control(){
+    float reference_alpha = getreferencebeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2);
+    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;
+    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){
+        motor1DirectionPin = 1;
+    }
+    else{
+        motor1DirectionPin = 0;
+    }
+}
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+
+
+
+
+
+//------------------------------------------------------------------------------
+//--------------------------------Motor2----------------------------------------
+//------------------------------------------------------------------------------
+PwmOut 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;
+double m2_err_int = 0;
+const double motor2_gain = 2*PI;
+
+
+void motor2_control(){
+    float reference_beta = getreferencealpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2);
+    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;
+    motor2 = fabs(magnitude2);
+    pc.printf("err2 = %f  beta = %f   mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
+    pc.printf("\r\n");
+    
+    if (magnitude2 > 0){
+        motor2DirectionPin = 1;
+    }
+    else{
+        motor2DirectionPin = 0;
+    }
+}
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+
+
+
+
+
+int main(){
+    pc.baud(115200);
+    motor1.period(0.0002f);
+    motor2.period(0.0002f);
+    MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
+    MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
+    But1.rise(&servo_control);
+     
+    
+    
+    
+           
+    while(1) {}   
+}
\ No newline at end of file