![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
sdfa
Dependencies: AnglePosition2 Encoder FastPWM MODSERIAL Movement PIDController Servo SignalNumber2 biquadFilter mbed
Fork of kinematics_controlv2 by
Diff: main.cpp
- Revision:
- 0:5f4bc2d63814
- Child:
- 1:406519ff0f17
diff -r 000000000000 -r 5f4bc2d63814 main.cpp --- /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