Floris Hoek / Mbed 2 deprecated template_biorobotics_Group_18

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL FastPWM

main.cpp@11:ca91fc47ad02, 2019-10-21 (annotated)

Committer:
paulstuiver
Date:
Mon Oct 21 14:20:28 2019 +0000
Revision:
11:ca91fc47ad02
Parent:
7:08fd3bc7a3cf
Child:
16:29d3851cfd52
atan2 was right after all

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RobertoO 0:67c50348f842 1 #include "mbed.h"
RobertoO 1:b862262a9d14 2 #include "MODSERIAL.h"
paulstuiver 2:75b2f713161c 3 #include "FastPWM.h"
paulstuiver 5:2ae500da8fe1 4 #include "QEI.h"
paulstuiver 5:2ae500da8fe1 5 #include <math.h>
paulstuiver 6:ea3b3f50c893 6 #include "BiQuad.h"
paulstuiver 2:75b2f713161c 7
paulstuiver 2:75b2f713161c 8 DigitalIn button1(D12);
paulstuiver 2:75b2f713161c 9 AnalogIn pot2(A0);
paulstuiver 7:08fd3bc7a3cf 10 AnalogIn pot1(A1);
paulstuiver 6:ea3b3f50c893 11 Ticker motor; //ticker to call motor values
paulstuiver 6:ea3b3f50c893 12 DigitalOut motor1Direction(D7); // is a boolean
paulstuiver 7:08fd3bc7a3cf 13 FastPWM motor1absolutemotorvalueocity(D6);
paulstuiver 6:ea3b3f50c893 14 MODSERIAL pc(USBTX, USBRX);
paulstuiver 5:2ae500da8fe1 15 QEI Encoder(D8,D9,NC,8400);
paulstuiver 7:08fd3bc7a3cf 16
paulstuiver 7:08fd3bc7a3cf 17
paulstuiver 2:75b2f713161c 18 volatile double frequency = 17000.0;
paulstuiver 2:75b2f713161c 19 volatile double period_signal = 1.0/frequency;
paulstuiver 7:08fd3bc7a3cf 20 float timeinterval = 0.001f;
paulstuiver 5:2ae500da8fe1 21 int counts;
paulstuiver 7:08fd3bc7a3cf 22 float measuredposition;
paulstuiver 6:ea3b3f50c893 23 float motorvalue;
paulstuiver 7:08fd3bc7a3cf 24 double u_i;
paulstuiver 7:08fd3bc7a3cf 25 float potmeter1 = pot1.read();
paulstuiver 11:ca91fc47ad02 26 float yendeffector = 20;
paulstuiver 11:ca91fc47ad02 27 float xendeffector = 0;
paulstuiver 11:ca91fc47ad02 28 float Kp;
paulstuiver 11:ca91fc47ad02 29
paulstuiver 7:08fd3bc7a3cf 30
paulstuiver 5:2ae500da8fe1 31
paulstuiver 5:2ae500da8fe1 32
paulstuiver 6:ea3b3f50c893 33
paulstuiver 6:ea3b3f50c893 34 // -------------------- README ------------------------------------
paulstuiver 6:ea3b3f50c893 35 // positive referenceposition corresponds to a counterclockwise motion
paulstuiver 6:ea3b3f50c893 36 // negative referenceposition corresponds to a clockwise motion
paulstuiver 6:ea3b3f50c893 37
paulstuiver 6:ea3b3f50c893 38 //PID control implementation (BiQuead)
paulstuiver 6:ea3b3f50c893 39 double PID_controller(double error)
paulstuiver 5:2ae500da8fe1 40 {
paulstuiver 6:ea3b3f50c893 41 static double error_integral = 0;
paulstuiver 6:ea3b3f50c893 42 static double error_prev = error;
paulstuiver 6:ea3b3f50c893 43 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
paulstuiver 11:ca91fc47ad02 44 float Kp = 30;
paulstuiver 11:ca91fc47ad02 45 float Ki = 2;
paulstuiver 11:ca91fc47ad02 46 float Kd = 0.6;
paulstuiver 6:ea3b3f50c893 47
paulstuiver 5:2ae500da8fe1 48 //Proportional part:
paulstuiver 5:2ae500da8fe1 49 double u_k = Kp * error;
paulstuiver 5:2ae500da8fe1 50
paulstuiver 6:ea3b3f50c893 51 // Integreal part
paulstuiver 6:ea3b3f50c893 52 error_integral = error_integral + error * timeinterval;
paulstuiver 6:ea3b3f50c893 53 double u_i = Ki*error_integral;
paulstuiver 7:08fd3bc7a3cf 54 // anti wind up
paulstuiver 7:08fd3bc7a3cf 55 if (u_i > 10)
paulstuiver 7:08fd3bc7a3cf 56 {
paulstuiver 7:08fd3bc7a3cf 57 u_i = 10 ;
paulstuiver 7:08fd3bc7a3cf 58 }
paulstuiver 6:ea3b3f50c893 59
paulstuiver 6:ea3b3f50c893 60 // Derivate part
paulstuiver 6:ea3b3f50c893 61 double error_derivative = (error - error_prev)/timeinterval;
paulstuiver 6:ea3b3f50c893 62 double filtered_error_derivative = LowPassFilter.step(error_derivative);
paulstuiver 6:ea3b3f50c893 63 double u_d = Kd * filtered_error_derivative;
paulstuiver 6:ea3b3f50c893 64 error_prev = error;
paulstuiver 6:ea3b3f50c893 65
paulstuiver 5:2ae500da8fe1 66 //sum all parts and return it
paulstuiver 6:ea3b3f50c893 67 return u_k + u_i + u_d;
paulstuiver 5:2ae500da8fe1 68 }
paulstuiver 2:75b2f713161c 69
paulstuiver 6:ea3b3f50c893 70 //get the measured position
paulstuiver 6:ea3b3f50c893 71 double getmeasuredposition()
paulstuiver 2:75b2f713161c 72 {
paulstuiver 5:2ae500da8fe1 73 counts = Encoder.getPulses();
paulstuiver 6:ea3b3f50c893 74 measuredposition = ((float)counts) / 8400.0f * 2.0f;
paulstuiver 6:ea3b3f50c893 75
paulstuiver 6:ea3b3f50c893 76 return measuredposition;
paulstuiver 2:75b2f713161c 77 }
paulstuiver 2:75b2f713161c 78
paulstuiver 7:08fd3bc7a3cf 79 //get the reference of the absolutemotorvalueocity
paulstuiver 6:ea3b3f50c893 80 double getreferenceposition()
paulstuiver 2:75b2f713161c 81 {
paulstuiver 11:ca91fc47ad02 82 float L0 = 1.95;
paulstuiver 11:ca91fc47ad02 83 float L1=15;
paulstuiver 11:ca91fc47ad02 84 float L2=20;
paulstuiver 11:ca91fc47ad02 85 float motorcounts1;
paulstuiver 11:ca91fc47ad02 86 float tangens = atan2(yendeffector,(L0+xendeffector))*180/3.1415;
paulstuiver 11:ca91fc47ad02 87 float cosinus = acos((pow(L1,2)+pow(L0+xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0+xendeffector,2)+pow(yendeffector,2))))*180/3.1415;
paulstuiver 11:ca91fc47ad02 88 motorcounts1=tangens+cosinus;
paulstuiver 11:ca91fc47ad02 89 //printf("motorcounts1 is %f\r\n", motorcounts1);
paulstuiver 11:ca91fc47ad02 90 float tangens2 = atan2(yendeffector,(L0-xendeffector))*180/3.1415;
paulstuiver 11:ca91fc47ad02 91 float cosinus2 = acos((pow(L1,2)+pow(L0-xendeffector,2)+pow(yendeffector,2)-pow(L2,2))/(2*L1*sqrt(pow(L0-xendeffector,2)+pow(yendeffector,2))))*180/3.1415;
paulstuiver 11:ca91fc47ad02 92 //motorcounts2=tangens2+cosinus2;
paulstuiver 7:08fd3bc7a3cf 93 float referenceposition;
paulstuiver 11:ca91fc47ad02 94 float variable;
paulstuiver 11:ca91fc47ad02 95 variable = motorcounts1/360;
paulstuiver 11:ca91fc47ad02 96 referenceposition = variable; //this determines the amount of spins
paulstuiver 6:ea3b3f50c893 97 return referenceposition;
paulstuiver 2:75b2f713161c 98 }
RobertoO 0:67c50348f842 99
paulstuiver 2:75b2f713161c 100 //send value to motor
paulstuiver 2:75b2f713161c 101 void sendtomotor(float motorvalue)
paulstuiver 2:75b2f713161c 102 {
paulstuiver 7:08fd3bc7a3cf 103 float absolutemotorvalue = 0.0f;
paulstuiver 7:08fd3bc7a3cf 104 absolutemotorvalue = fabs(motorvalue);
paulstuiver 7:08fd3bc7a3cf 105 absolutemotorvalue = absolutemotorvalue > 1.0f ? 1.0f : absolutemotorvalue; //if absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
paulstuiver 7:08fd3bc7a3cf 106 motor1absolutemotorvalueocity = absolutemotorvalue;
RobertoO 0:67c50348f842 107
paulstuiver 6:ea3b3f50c893 108 motor1Direction = (motorvalue > 0.0f); //boolean output: true gives counterclockwise direction, false gives clockwise direction
paulstuiver 2:75b2f713161c 109 }
RobertoO 0:67c50348f842 110
paulstuiver 7:08fd3bc7a3cf 111 // function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
paulstuiver 2:75b2f713161c 112 void measureandcontrol(void)
paulstuiver 2:75b2f713161c 113 {
paulstuiver 11:ca91fc47ad02 114 float referenceposition = getreferenceposition();
paulstuiver 6:ea3b3f50c893 115 measuredposition = getmeasuredposition();
paulstuiver 6:ea3b3f50c893 116 motorvalue = PID_controller(referenceposition - measuredposition);
paulstuiver 6:ea3b3f50c893 117 sendtomotor(motorvalue);
paulstuiver 2:75b2f713161c 118 }
RobertoO 0:67c50348f842 119 int main()
RobertoO 0:67c50348f842 120 {
RobertoO 0:67c50348f842 121 pc.baud(115200);
paulstuiver 2:75b2f713161c 122 pc.printf("Starting...\r\n");
paulstuiver 7:08fd3bc7a3cf 123 motor1absolutemotorvalueocity.period(period_signal);
paulstuiver 6:ea3b3f50c893 124 motor.attach(measureandcontrol, timeinterval);
paulstuiver 2:75b2f713161c 125 while(true)
paulstuiver 2:75b2f713161c 126 {
paulstuiver 2:75b2f713161c 127 wait(0.5);
paulstuiver 5:2ae500da8fe1 128 pc.printf("number of counts %i\r\n", counts);
paulstuiver 6:ea3b3f50c893 129 pc.printf("measured position is %f\r\n", measuredposition);
paulstuiver 6:ea3b3f50c893 130 pc.printf("motorvalue is %f\r\n", motorvalue);
paulstuiver 7:08fd3bc7a3cf 131 pc.printf("u_i is %d\r\n", u_i);
paulstuiver 7:08fd3bc7a3cf 132 pc.printf("potmeter 1 gives %f\r\n", potmeter1);
paulstuiver 11:ca91fc47ad02 133 pc.printf("x is %f\r\n",xendeffector);
paulstuiver 11:ca91fc47ad02 134 pc.printf("y is %f\r\n",yendeffector);
paulstuiver 11:ca91fc47ad02 135 pc.printf("Kp is %f\r\n", Kp);
paulstuiver 11:ca91fc47ad02 136
paulstuiver 11:ca91fc47ad02 137
paulstuiver 11:ca91fc47ad02 138 char a = pc.getc();
paulstuiver 11:ca91fc47ad02 139
paulstuiver 11:ca91fc47ad02 140 if(a == 'r'){
paulstuiver 11:ca91fc47ad02 141 xendeffector=xendeffector+5;}
paulstuiver 11:ca91fc47ad02 142 else if(a=='l'){
paulstuiver 11:ca91fc47ad02 143 xendeffector=xendeffector-5;}
paulstuiver 11:ca91fc47ad02 144 else if(a=='u'){
paulstuiver 11:ca91fc47ad02 145 yendeffector=yendeffector+5;}
paulstuiver 11:ca91fc47ad02 146 else if(a=='d'){
paulstuiver 11:ca91fc47ad02 147 yendeffector=yendeffector-5;}
paulstuiver 11:ca91fc47ad02 148
paulstuiver 11:ca91fc47ad02 149
paulstuiver 6:ea3b3f50c893 150
paulstuiver 6:ea3b3f50c893 151 char c = pc.getcNb();
paulstuiver 6:ea3b3f50c893 152
paulstuiver 7:08fd3bc7a3cf 153 // type c to stop the program
paulstuiver 6:ea3b3f50c893 154 if (c == 'c')
paulstuiver 6:ea3b3f50c893 155 {
paulstuiver 7:08fd3bc7a3cf 156 pc.printf("Program stopped running");
paulstuiver 6:ea3b3f50c893 157 motorvalue = 0;
paulstuiver 6:ea3b3f50c893 158 sendtomotor(motorvalue);
paulstuiver 7:08fd3bc7a3cf 159 break;
paulstuiver 6:ea3b3f50c893 160 }
RobertoO 0:67c50348f842 161 }
paulstuiver 2:75b2f713161c 162 }