Peter Knoben / Mbed 2 deprecated kinematics_control

Kinematics inclusief headers

Dependencies:   AnglePosition Encoder FastPWM MODSERIAL PIDController Servo mbed

Fork of kinematics_control by Dustin Berendsen

main.cpp@2:2ad485d762f5, 2017-10-26 (annotated)

Committer:
peterknoben
Date:
Thu Oct 26 10:54:56 2017 +0000
Revision:
2:2ad485d762f5
Parent:
1:406519ff0f17 
Kinematics inclusief headers

Who changed what in which revision?

UserRevisionLine numberNew contents of line
peterknoben 1:406519ff0f17 1 #include "AnglePosition.h"
peterknoben 1:406519ff0f17 2 #include "PIDControl.h"
DBerendsen 0:5f4bc2d63814 3 #include "mbed.h"
DBerendsen 0:5f4bc2d63814 4 #include "encoder.h"
DBerendsen 0:5f4bc2d63814 5 #include "Servo.h"
DBerendsen 0:5f4bc2d63814 6 #include "MODSERIAL.h"
peterknoben 1:406519ff0f17 7 #include "FastPWM.h"
DBerendsen 0:5f4bc2d63814 8
DBerendsen 0:5f4bc2d63814 9 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 10 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 11 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 12 MODSERIAL pc(USBTX, USBRX); //
peterknoben 1:406519ff0f17 13 Ticker MyControllerTicker1; //Declare Ticker Motor 1
peterknoben 1:406519ff0f17 14 Ticker MyControllerTicker2; //Declare Ticker Motor 2
peterknoben 1:406519ff0f17 15 AnglePosition Angle; //Declare Angle calculater
peterknoben 1:406519ff0f17 16 PIDControl PID; //Declare PID Controller
peterknoben 1:406519ff0f17 17 AnalogIn potmeter1(A3); //Set Inputpin
peterknoben 1:406519ff0f17 18 AnalogIn potmeter2(A4); //Set Inputpin
peterknoben 1:406519ff0f17 19
peterknoben 1:406519ff0f17 20 const float CONTROLLER_TS = 0.02; //Motor frequency
peterknoben 1:406519ff0f17 21
peterknoben 1:406519ff0f17 22 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 23 //-------------------------Kinematic Constants----------------------------------
peterknoben 1:406519ff0f17 24 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 25 const double RAD_PER_PULSE = 0.00074799825*2;
DBerendsen 0:5f4bc2d63814 26 const double PI = 3.14159265358979323846;
peterknoben 1:406519ff0f17 27 const float max_rangex = 500.0;
peterknoben 1:406519ff0f17 28 const float max_rangey = 300.0;
peterknoben 1:406519ff0f17 29 const float x_offset = 156.15;
peterknoben 1:406519ff0f17 30 const float y_offset = -76.97;
peterknoben 1:406519ff0f17 31 const float alpha_offset = -(21.52/180)*PI;
peterknoben 1:406519ff0f17 32 const float beta_offset = 0.0;
peterknoben 1:406519ff0f17 33 const float L1 = 450.0;
peterknoben 1:406519ff0f17 34 const float L2 = 490.0;
DBerendsen 0:5f4bc2d63814 35
DBerendsen 0:5f4bc2d63814 36
DBerendsen 0:5f4bc2d63814 37 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 38 //--------------------------------Servo-----------------------------------------
DBerendsen 0:5f4bc2d63814 39 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 40 Servo MyServo(D9); //Declare button
peterknoben 1:406519ff0f17 41 InterruptIn But1(D8); //Declare used button
peterknoben 1:406519ff0f17 42 int k=0; //Position flag
DBerendsen 0:5f4bc2d63814 43
DBerendsen 0:5f4bc2d63814 44 void servo_control (){
DBerendsen 0:5f4bc2d63814 45 if (k==0){
DBerendsen 0:5f4bc2d63814 46 MyServo = 0;
DBerendsen 0:5f4bc2d63814 47 k=1;
DBerendsen 0:5f4bc2d63814 48 }
DBerendsen 0:5f4bc2d63814 49 else{
DBerendsen 0:5f4bc2d63814 50 MyServo = 2;
DBerendsen 0:5f4bc2d63814 51 k=0;
peterknoben 1:406519ff0f17 52 }
DBerendsen 0:5f4bc2d63814 53 }
DBerendsen 0:5f4bc2d63814 54
DBerendsen 0:5f4bc2d63814 55
DBerendsen 0:5f4bc2d63814 56 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 57 //--------------------------------Motor1----------------------------------------
DBerendsen 0:5f4bc2d63814 58 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 59 FastPWM motor1(D5);
DBerendsen 0:5f4bc2d63814 60 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:5f4bc2d63814 61 DigitalIn ENC2A(D12);
DBerendsen 0:5f4bc2d63814 62 DigitalIn ENC2B(D13);
DBerendsen 0:5f4bc2d63814 63 Encoder encoder1(D13,D12);
peterknoben 1:406519ff0f17 64 const float MOTOR1_KP = 40.0;
peterknoben 1:406519ff0f17 65 const float MOTOR1_KI = 0.0;
peterknoben 1:406519ff0f17 66 const float MOTOR1_KD = 15.0;
peterknoben 1:406519ff0f17 67 double M1_v1 = 0.0;
peterknoben 1:406519ff0f17 68 double M1_v2 = 0.0;
DBerendsen 0:5f4bc2d63814 69 const double motor1_gain = 2*PI;
peterknoben 1:406519ff0f17 70 const float M1_N = 0.5;
DBerendsen 0:5f4bc2d63814 71
DBerendsen 0:5f4bc2d63814 72
DBerendsen 0:5f4bc2d63814 73 void motor1_control(){
peterknoben 1:406519ff0f17 74 float reference_alpha = Angle.getbeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2, potmeter1, potmeter2);
DBerendsen 0:5f4bc2d63814 75 float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:5f4bc2d63814 76 float error_alpha = reference_alpha-position_alpha;
peterknoben 1:406519ff0f17 77 float magnitude1 = PID.get(error_alpha, MOTOR1_KP, MOTOR1_KI, MOTOR1_KD, CONTROLLER_TS, M1_N, M1_v1, M1_v2) / motor1_gain;
DBerendsen 0:5f4bc2d63814 78 motor1 = fabs(magnitude1);
DBerendsen 0:5f4bc2d63814 79 pc.printf("err_a = %f alpha = %f mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
DBerendsen 0:5f4bc2d63814 80 pc.printf("\r\n");
peterknoben 1:406519ff0f17 81 // Determine Motor Direction
peterknoben 1:406519ff0f17 82 if (magnitude1 < 0){
DBerendsen 0:5f4bc2d63814 83 motor1DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 84 }
DBerendsen 0:5f4bc2d63814 85 else{
DBerendsen 0:5f4bc2d63814 86 motor1DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 87 }
DBerendsen 0:5f4bc2d63814 88 }
DBerendsen 0:5f4bc2d63814 89
DBerendsen 0:5f4bc2d63814 90 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 91 //--------------------------------Motor2----------------------------------------
DBerendsen 0:5f4bc2d63814 92 //------------------------------------------------------------------------------
peterknoben 1:406519ff0f17 93 FastPWM motor2(D6);
DBerendsen 0:5f4bc2d63814 94 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:5f4bc2d63814 95 DigitalIn ENC1A(D10);
DBerendsen 0:5f4bc2d63814 96 DigitalIn ENC1B(D11);
DBerendsen 0:5f4bc2d63814 97 Encoder encoder2(D10,D11);
peterknoben 1:406519ff0f17 98 const float MOTOR2_KP = 60.0;
peterknoben 1:406519ff0f17 99 const float MOTOR2_KI = 0.0;
peterknoben 1:406519ff0f17 100 const float MOTOR2_KD = 15.0;
DBerendsen 0:5f4bc2d63814 101 double m2_err_int = 0;
DBerendsen 0:5f4bc2d63814 102 const double motor2_gain = 2*PI;
peterknoben 1:406519ff0f17 103 const float M2_N = 0.5;
peterknoben 1:406519ff0f17 104 double M2_v1 = 0.0;
peterknoben 1:406519ff0f17 105 double M2_v2 = 0.0;
DBerendsen 0:5f4bc2d63814 106
DBerendsen 0:5f4bc2d63814 107
DBerendsen 0:5f4bc2d63814 108 void motor2_control(){
peterknoben 1:406519ff0f17 109 float reference_beta = Angle.getalpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2, potmeter1, potmeter2);
DBerendsen 0:5f4bc2d63814 110 float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
DBerendsen 0:5f4bc2d63814 111 float error_beta = reference_beta-position_beta;
peterknoben 1:406519ff0f17 112 float magnitude2 = PID.get(error_beta, MOTOR2_KP, MOTOR2_KI, MOTOR2_KD, CONTROLLER_TS, M2_N, M1_v1, M1_v2) / motor2_gain;
DBerendsen 0:5f4bc2d63814 113 motor2 = fabs(magnitude2);
DBerendsen 0:5f4bc2d63814 114 pc.printf("err2 = %f beta = %f mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
DBerendsen 0:5f4bc2d63814 115 pc.printf("\r\n");
peterknoben 1:406519ff0f17 116 //Determine Motor Direction
DBerendsen 0:5f4bc2d63814 117 if (magnitude2 > 0){
DBerendsen 0:5f4bc2d63814 118 motor2DirectionPin = 1;
DBerendsen 0:5f4bc2d63814 119 }
DBerendsen 0:5f4bc2d63814 120 else{
DBerendsen 0:5f4bc2d63814 121 motor2DirectionPin = 0;
DBerendsen 0:5f4bc2d63814 122 }
DBerendsen 0:5f4bc2d63814 123 }
DBerendsen 0:5f4bc2d63814 124 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 125 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 126 //------------------------------------------------------------------------------
DBerendsen 0:5f4bc2d63814 127
DBerendsen 0:5f4bc2d63814 128 int main(){
DBerendsen 0:5f4bc2d63814 129 pc.baud(115200);
peterknoben 1:406519ff0f17 130 motor1.period(0.0001f);
peterknoben 1:406519ff0f17 131 motor2.period(0.0001f);
DBerendsen 0:5f4bc2d63814 132 MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);
DBerendsen 0:5f4bc2d63814 133 MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
peterknoben 1:406519ff0f17 134 But1.rise(&servo_control);
DBerendsen 0:5f4bc2d63814 135 while(1) {}
peterknoben 1:406519ff0f17 136 }
peterknoben 1:406519ff0f17 137
peterknoben 1:406519ff0f17 138