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@7:08fd3bc7a3cf, 2019-10-11 (annotated)

Committer:
paulstuiver
Date:
Fri Oct 11 13:40:36 2019 +0000
Revision:
7:08fd3bc7a3cf
Parent:
6:ea3b3f50c893
Child:
11:ca91fc47ad02
assignment graded RKI; ; ; ; ; jokesss xd 69, Kp depends on pot1

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 7:08fd3bc7a3cf 26
paulstuiver 5:2ae500da8fe1 27
paulstuiver 5:2ae500da8fe1 28
paulstuiver 6:ea3b3f50c893 29
paulstuiver 6:ea3b3f50c893 30 // -------------------- README ------------------------------------
paulstuiver 6:ea3b3f50c893 31 // positive referenceposition corresponds to a counterclockwise motion
paulstuiver 6:ea3b3f50c893 32 // negative referenceposition corresponds to a clockwise motion
paulstuiver 6:ea3b3f50c893 33
paulstuiver 6:ea3b3f50c893 34 //PID control implementation (BiQuead)
paulstuiver 6:ea3b3f50c893 35 double PID_controller(double error)
paulstuiver 5:2ae500da8fe1 36 {
paulstuiver 6:ea3b3f50c893 37 static double error_integral = 0;
paulstuiver 6:ea3b3f50c893 38 static double error_prev = error;
paulstuiver 6:ea3b3f50c893 39 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
paulstuiver 7:08fd3bc7a3cf 40 double Kp = pot1.read()*10;
paulstuiver 7:08fd3bc7a3cf 41 double Ki = 0.1;
paulstuiver 7:08fd3bc7a3cf 42 double Kd = 0.1;
paulstuiver 6:ea3b3f50c893 43
paulstuiver 5:2ae500da8fe1 44 //Proportional part:
paulstuiver 5:2ae500da8fe1 45 double u_k = Kp * error;
paulstuiver 5:2ae500da8fe1 46
paulstuiver 6:ea3b3f50c893 47 // Integreal part
paulstuiver 6:ea3b3f50c893 48 error_integral = error_integral + error * timeinterval;
paulstuiver 6:ea3b3f50c893 49 double u_i = Ki*error_integral;
paulstuiver 7:08fd3bc7a3cf 50 // anti wind up
paulstuiver 7:08fd3bc7a3cf 51 if (u_i > 10)
paulstuiver 7:08fd3bc7a3cf 52 {
paulstuiver 7:08fd3bc7a3cf 53 u_i = 10 ;
paulstuiver 7:08fd3bc7a3cf 54 }
paulstuiver 6:ea3b3f50c893 55
paulstuiver 6:ea3b3f50c893 56 // Derivate part
paulstuiver 6:ea3b3f50c893 57 double error_derivative = (error - error_prev)/timeinterval;
paulstuiver 6:ea3b3f50c893 58 double filtered_error_derivative = LowPassFilter.step(error_derivative);
paulstuiver 6:ea3b3f50c893 59 double u_d = Kd * filtered_error_derivative;
paulstuiver 6:ea3b3f50c893 60 error_prev = error;
paulstuiver 6:ea3b3f50c893 61
paulstuiver 5:2ae500da8fe1 62 //sum all parts and return it
paulstuiver 6:ea3b3f50c893 63 return u_k + u_i + u_d;
paulstuiver 5:2ae500da8fe1 64 }
paulstuiver 2:75b2f713161c 65
paulstuiver 6:ea3b3f50c893 66 //get the measured position
paulstuiver 6:ea3b3f50c893 67 double getmeasuredposition()
paulstuiver 2:75b2f713161c 68 {
paulstuiver 5:2ae500da8fe1 69 counts = Encoder.getPulses();
paulstuiver 6:ea3b3f50c893 70 measuredposition = ((float)counts) / 8400.0f * 2.0f;
paulstuiver 6:ea3b3f50c893 71
paulstuiver 6:ea3b3f50c893 72 return measuredposition;
paulstuiver 2:75b2f713161c 73 }
paulstuiver 2:75b2f713161c 74
paulstuiver 7:08fd3bc7a3cf 75 //get the reference of the absolutemotorvalueocity
paulstuiver 6:ea3b3f50c893 76 double getreferenceposition()
paulstuiver 2:75b2f713161c 77 {
paulstuiver 7:08fd3bc7a3cf 78 float referenceposition;
paulstuiver 7:08fd3bc7a3cf 79 referenceposition = -1+2*pot2.read(); //this determines the amount of spins
paulstuiver 6:ea3b3f50c893 80 return referenceposition;
paulstuiver 2:75b2f713161c 81 }
RobertoO 0:67c50348f842 82
paulstuiver 2:75b2f713161c 83 //send value to motor
paulstuiver 2:75b2f713161c 84 void sendtomotor(float motorvalue)
paulstuiver 2:75b2f713161c 85 {
paulstuiver 7:08fd3bc7a3cf 86 float absolutemotorvalue = 0.0f;
paulstuiver 7:08fd3bc7a3cf 87 absolutemotorvalue = fabs(motorvalue);
paulstuiver 7:08fd3bc7a3cf 88 absolutemotorvalue = absolutemotorvalue > 1.0f ? 1.0f : absolutemotorvalue; //if absolutemotorvalueocity is greater than 1, reduce to 1, otherwise remain absolutemotorvalue
paulstuiver 7:08fd3bc7a3cf 89 motor1absolutemotorvalueocity = absolutemotorvalue;
RobertoO 0:67c50348f842 90
paulstuiver 6:ea3b3f50c893 91 motor1Direction = (motorvalue > 0.0f); //boolean output: true gives counterclockwise direction, false gives clockwise direction
paulstuiver 2:75b2f713161c 92 }
RobertoO 0:67c50348f842 93
paulstuiver 7:08fd3bc7a3cf 94 // function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
paulstuiver 2:75b2f713161c 95 void measureandcontrol(void)
paulstuiver 2:75b2f713161c 96 {
paulstuiver 6:ea3b3f50c893 97 float referenceposition = getreferenceposition();
paulstuiver 6:ea3b3f50c893 98 measuredposition = getmeasuredposition();
paulstuiver 6:ea3b3f50c893 99 motorvalue = PID_controller(referenceposition - measuredposition);
paulstuiver 6:ea3b3f50c893 100 sendtomotor(motorvalue);
paulstuiver 2:75b2f713161c 101 }
RobertoO 0:67c50348f842 102 int main()
RobertoO 0:67c50348f842 103 {
RobertoO 0:67c50348f842 104 pc.baud(115200);
paulstuiver 2:75b2f713161c 105 pc.printf("Starting...\r\n");
paulstuiver 7:08fd3bc7a3cf 106 motor1absolutemotorvalueocity.period(period_signal);
paulstuiver 6:ea3b3f50c893 107 motor.attach(measureandcontrol, timeinterval);
paulstuiver 2:75b2f713161c 108 while(true)
paulstuiver 2:75b2f713161c 109 {
paulstuiver 2:75b2f713161c 110 wait(0.5);
paulstuiver 5:2ae500da8fe1 111 pc.printf("number of counts %i\r\n", counts);
paulstuiver 6:ea3b3f50c893 112 pc.printf("measured position is %f\r\n", measuredposition);
paulstuiver 6:ea3b3f50c893 113 pc.printf("motorvalue is %f\r\n", motorvalue);
paulstuiver 7:08fd3bc7a3cf 114 pc.printf("u_i is %d\r\n", u_i);
paulstuiver 7:08fd3bc7a3cf 115 pc.printf("potmeter 1 gives %f\r\n", potmeter1);
paulstuiver 6:ea3b3f50c893 116
paulstuiver 6:ea3b3f50c893 117 char c = pc.getcNb();
paulstuiver 6:ea3b3f50c893 118
paulstuiver 7:08fd3bc7a3cf 119 // type c to stop the program
paulstuiver 6:ea3b3f50c893 120 if (c == 'c')
paulstuiver 6:ea3b3f50c893 121 {
paulstuiver 7:08fd3bc7a3cf 122 pc.printf("Program stopped running");
paulstuiver 6:ea3b3f50c893 123 motorvalue = 0;
paulstuiver 6:ea3b3f50c893 124 sendtomotor(motorvalue);
paulstuiver 7:08fd3bc7a3cf 125 break;
paulstuiver 6:ea3b3f50c893 126 }
RobertoO 0:67c50348f842 127 }
paulstuiver 2:75b2f713161c 128 }