Peter Knoben / Mbed 2 deprecated Angle_control_v3

Angle control and Servo control with kinematics

Dependencies:   Encoder FastPWM Servo mbed

Fork of Angle_control_v2 by Peter Knoben

main.cpp@4:d385669b2f50, 2017-10-17 (annotated)

Committer:
peterknoben
Date:
Tue Oct 17 09:31:20 2017 +0000
Revision:
4:d385669b2f50
Parent:
3:1514725c8cc8
Child:
5:b4ec742aa7d4
Versie voor x en y bepaling;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DBerendsen 0:bbd4e22aca8a 1 #include "mbed.h"
DBerendsen 0:bbd4e22aca8a 2 #include "encoder.h"
peterknoben 2:47ec66bbe8f9 3 #include "Servo.h"
peterknoben 3:1514725c8cc8 4 #include "FastPWM.h"
DBerendsen 0:bbd4e22aca8a 5
peterknoben 2:47ec66bbe8f9 6 Ticker MyControllerTicker1;
peterknoben 2:47ec66bbe8f9 7 Ticker MyControllerTicker2;
DBerendsen 0:bbd4e22aca8a 8 const double PI = 3.141592653589793;
peterknoben 3:1514725c8cc8 9 const double RAD_PER_PULSE = 0.000749425;
peterknoben 2:47ec66bbe8f9 10 const double CONTROLLER_TS = 0.01;
DBerendsen 0:bbd4e22aca8a 11
DBerendsen 0:bbd4e22aca8a 12
DBerendsen 0:bbd4e22aca8a 13 //Motor1
DBerendsen 0:bbd4e22aca8a 14 PwmOut motor1(D5);
DBerendsen 0:bbd4e22aca8a 15 DigitalOut motor1DirectionPin(D4);
DBerendsen 0:bbd4e22aca8a 16 DigitalIn ENC2A(D12);
DBerendsen 0:bbd4e22aca8a 17 DigitalIn ENC2B(D13);
DBerendsen 0:bbd4e22aca8a 18 Encoder encoder1(D13,D12);
DBerendsen 0:bbd4e22aca8a 19 AnalogIn potmeter1(A3);
peterknoben 3:1514725c8cc8 20 const double MOTOR1_KP = 15;
peterknoben 3:1514725c8cc8 21 const double MOTOR1_KI = 10;
DBerendsen 0:bbd4e22aca8a 22 double m1_err_int = 0;
DBerendsen 0:bbd4e22aca8a 23 const double motor1_gain = 2*PI;
DBerendsen 0:bbd4e22aca8a 24
DBerendsen 0:bbd4e22aca8a 25
DBerendsen 0:bbd4e22aca8a 26 //Motor2
DBerendsen 0:bbd4e22aca8a 27 PwmOut motor2(D6);
DBerendsen 0:bbd4e22aca8a 28 DigitalOut motor2DirectionPin(D7);
DBerendsen 0:bbd4e22aca8a 29 DigitalIn ENC1A(D10);
DBerendsen 0:bbd4e22aca8a 30 DigitalIn ENC1B(D11);
DBerendsen 0:bbd4e22aca8a 31 Encoder encoder2(D10,D11);
DBerendsen 0:bbd4e22aca8a 32 AnalogIn potmeter2(A4);
peterknoben 3:1514725c8cc8 33 const double MOTOR2_KP = 15;
peterknoben 3:1514725c8cc8 34 const double MOTOR2_KI = 10;
DBerendsen 0:bbd4e22aca8a 35 double m2_err_int = 0;
DBerendsen 0:bbd4e22aca8a 36 const double motor2_gain = 2*PI;
DBerendsen 0:bbd4e22aca8a 37
peterknoben 3:1514725c8cc8 38
peterknoben 2:47ec66bbe8f9 39 //Servo
peterknoben 2:47ec66bbe8f9 40 Servo MyServo(D9);
peterknoben 2:47ec66bbe8f9 41 InterruptIn But1(D8);
peterknoben 2:47ec66bbe8f9 42 int k=0;
DBerendsen 0:bbd4e22aca8a 43
peterknoben 2:47ec66bbe8f9 44
peterknoben 4:d385669b2f50 45 float getreferenceangle(const double PI, double potmeter){
DBerendsen 0:bbd4e22aca8a 46 float max_angle = 2*PI;
DBerendsen 0:bbd4e22aca8a 47 float reference_angle = max_angle * potmeter;
DBerendsen 0:bbd4e22aca8a 48 return reference_angle;
peterknoben 4:d385669b2f50 49 }
DBerendsen 0:bbd4e22aca8a 50
peterknoben 2:47ec66bbe8f9 51 double PI_controller(double error, const double Kp, const double Ki, double Ts, double &e_int) {
DBerendsen 0:bbd4e22aca8a 52 e_int =+ Ts * error;
DBerendsen 0:bbd4e22aca8a 53 return Kp * error + Ki * e_int ;
DBerendsen 0:bbd4e22aca8a 54 }
DBerendsen 0:bbd4e22aca8a 55
peterknoben 2:47ec66bbe8f9 56 void motor1_control(){
DBerendsen 0:bbd4e22aca8a 57 double referenceangle1 = getreferenceangle(PI, potmeter1);
DBerendsen 0:bbd4e22aca8a 58 double position1 = RAD_PER_PULSE * encoder1.getPosition();
DBerendsen 0:bbd4e22aca8a 59 double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain;
DBerendsen 0:bbd4e22aca8a 60 motor1 = fabs(magnitude1);
DBerendsen 0:bbd4e22aca8a 61
peterknoben 2:47ec66bbe8f9 62 if (magnitude1 < 0){
DBerendsen 0:bbd4e22aca8a 63 motor1DirectionPin = 1;
DBerendsen 0:bbd4e22aca8a 64 }
peterknoben 2:47ec66bbe8f9 65 else{
DBerendsen 0:bbd4e22aca8a 66 motor1DirectionPin = 0;
DBerendsen 0:bbd4e22aca8a 67 }
DBerendsen 0:bbd4e22aca8a 68 }
DBerendsen 0:bbd4e22aca8a 69
peterknoben 2:47ec66bbe8f9 70
peterknoben 2:47ec66bbe8f9 71 void motor2_control(){
DBerendsen 0:bbd4e22aca8a 72 double referenceangle2 = getreferenceangle(PI, potmeter2);
DBerendsen 0:bbd4e22aca8a 73 double position2 = RAD_PER_PULSE * encoder2.getPosition();
DBerendsen 0:bbd4e22aca8a 74 double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain;
DBerendsen 1:5681fcdc22fe 75 motor2 = fabs(magnitude2);
DBerendsen 0:bbd4e22aca8a 76
peterknoben 2:47ec66bbe8f9 77 if (magnitude2 < 0){
DBerendsen 1:5681fcdc22fe 78 motor2DirectionPin = 1;
DBerendsen 0:bbd4e22aca8a 79 }
peterknoben 2:47ec66bbe8f9 80 else{
DBerendsen 1:5681fcdc22fe 81 motor2DirectionPin = 0;
DBerendsen 0:bbd4e22aca8a 82 }
DBerendsen 0:bbd4e22aca8a 83 }
DBerendsen 0:bbd4e22aca8a 84
peterknoben 4:d385669b2f50 85 void servo_control (){
peterknoben 3:1514725c8cc8 86 motor1.period(0.02f);
peterknoben 3:1514725c8cc8 87 motor2.period(0.02f);
peterknoben 2:47ec66bbe8f9 88 if (k==0){
peterknoben 2:47ec66bbe8f9 89 MyServo = 0;
peterknoben 2:47ec66bbe8f9 90 k=1;
peterknoben 3:1514725c8cc8 91 motor1.period_us(200);
peterknoben 2:47ec66bbe8f9 92 }
peterknoben 2:47ec66bbe8f9 93 else{
peterknoben 2:47ec66bbe8f9 94 MyServo = 2;
peterknoben 2:47ec66bbe8f9 95 k=0;
peterknoben 3:1514725c8cc8 96 motor1.period_us(200);
peterknoben 3:1514725c8cc8 97 }
peterknoben 4:d385669b2f50 98 }
DBerendsen 0:bbd4e22aca8a 99
peterknoben 2:47ec66bbe8f9 100
peterknoben 2:47ec66bbe8f9 101 int main(){
peterknoben 3:1514725c8cc8 102 But1.rise(&servo_control);
peterknoben 3:1514725c8cc8 103 motor1.period(0.0002f);
peterknoben 3:1514725c8cc8 104 motor2.period(0.0002f);
peterknoben 2:47ec66bbe8f9 105 MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS);
peterknoben 2:47ec66bbe8f9 106 MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
peterknoben 3:1514725c8cc8 107
peterknoben 2:47ec66bbe8f9 108
peterknoben 2:47ec66bbe8f9 109 while(1) {}
peterknoben 3:1514725c8cc8 110 }