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

Committer:
paulstuiver
Date:
Fri Oct 11 12:42:36 2019 +0000
Revision:
6:ea3b3f50c893
Parent:
5:2ae500da8fe1
Child:
7:08fd3bc7a3cf
position inputs with feedback instead of velocity inputs

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 6:ea3b3f50c893 10 Ticker motor; //ticker to call motor values
paulstuiver 6:ea3b3f50c893 11 DigitalOut motor1Direction(D7); // is a boolean
paulstuiver 3:4f281c336a8b 12 FastPWM motor1Velocity(D6);
paulstuiver 6:ea3b3f50c893 13 MODSERIAL pc(USBTX, USBRX);
paulstuiver 5:2ae500da8fe1 14 QEI Encoder(D8,D9,NC,8400);
paulstuiver 2:75b2f713161c 15 volatile double frequency = 17000.0;
paulstuiver 2:75b2f713161c 16 volatile double period_signal = 1.0/frequency;
paulstuiver 3:4f281c336a8b 17 float vel = 0.0f;
paulstuiver 3:4f281c336a8b 18 float referencevelocity;
paulstuiver 3:4f281c336a8b 19 float motorvalue2;
paulstuiver 5:2ae500da8fe1 20 int counts;
paulstuiver 5:2ae500da8fe1 21 float position1 = 0;
paulstuiver 5:2ae500da8fe1 22 float position2;
paulstuiver 6:ea3b3f50c893 23 float position11;
paulstuiver 6:ea3b3f50c893 24 float position22;
paulstuiver 6:ea3b3f50c893 25 float timeinterval = 0.001f;
paulstuiver 5:2ae500da8fe1 26 float measuredvelocity;
paulstuiver 6:ea3b3f50c893 27 double Kp = 5;
paulstuiver 6:ea3b3f50c893 28 double Ki = 0.1;
paulstuiver 6:ea3b3f50c893 29 double Kd = 1;
paulstuiver 6:ea3b3f50c893 30 float motorvalue;
paulstuiver 6:ea3b3f50c893 31 float desiredposition;
paulstuiver 6:ea3b3f50c893 32 float measuredposition;
paulstuiver 6:ea3b3f50c893 33 float referenceposition;
paulstuiver 5:2ae500da8fe1 34
paulstuiver 5:2ae500da8fe1 35
paulstuiver 6:ea3b3f50c893 36
paulstuiver 6:ea3b3f50c893 37 // -------------------- README ------------------------------------
paulstuiver 6:ea3b3f50c893 38 // positive referenceposition corresponds to a counterclockwise motion
paulstuiver 6:ea3b3f50c893 39 // negative referenceposition corresponds to a clockwise motion
paulstuiver 6:ea3b3f50c893 40
paulstuiver 6:ea3b3f50c893 41 //PID control implementation (BiQuead)
paulstuiver 6:ea3b3f50c893 42 double PID_controller(double error)
paulstuiver 5:2ae500da8fe1 43 {
paulstuiver 6:ea3b3f50c893 44 static double error_integral = 0;
paulstuiver 6:ea3b3f50c893 45 static double error_prev = error;
paulstuiver 6:ea3b3f50c893 46 static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
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 6:ea3b3f50c893 54
paulstuiver 6:ea3b3f50c893 55 // Derivate part
paulstuiver 6:ea3b3f50c893 56 double error_derivative = (error - error_prev)/timeinterval;
paulstuiver 6:ea3b3f50c893 57 double filtered_error_derivative = LowPassFilter.step(error_derivative);
paulstuiver 6:ea3b3f50c893 58 double u_d = Kd * filtered_error_derivative;
paulstuiver 6:ea3b3f50c893 59 error_prev = error;
paulstuiver 6:ea3b3f50c893 60
paulstuiver 5:2ae500da8fe1 61 //sum all parts and return it
paulstuiver 6:ea3b3f50c893 62 return u_k + u_i + u_d;
paulstuiver 5:2ae500da8fe1 63 }
paulstuiver 2:75b2f713161c 64
paulstuiver 6:ea3b3f50c893 65 //get the measured position
paulstuiver 6:ea3b3f50c893 66 double getmeasuredposition()
paulstuiver 2:75b2f713161c 67 {
paulstuiver 5:2ae500da8fe1 68 counts = Encoder.getPulses();
paulstuiver 5:2ae500da8fe1 69 counts = counts % 8400;
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 6:ea3b3f50c893 75 //get the reference of the velocity
paulstuiver 6:ea3b3f50c893 76 double getreferenceposition()
paulstuiver 2:75b2f713161c 77 {
paulstuiver 6:ea3b3f50c893 78 referenceposition = 1; //this determines the amount of spins
paulstuiver 6:ea3b3f50c893 79 return referenceposition;
paulstuiver 2:75b2f713161c 80 }
RobertoO 0:67c50348f842 81
paulstuiver 2:75b2f713161c 82 //send value to motor
paulstuiver 2:75b2f713161c 83 void sendtomotor(float motorvalue)
paulstuiver 2:75b2f713161c 84 {
paulstuiver 3:4f281c336a8b 85 motorvalue2 = motorvalue;
paulstuiver 2:75b2f713161c 86 vel = fabs(motorvalue);
paulstuiver 6:ea3b3f50c893 87 vel = vel > 1.0f ? 1.0f : vel; //if velocity is greater than 1, reduce to 1, otherwise remain vel
paulstuiver 2:75b2f713161c 88 motor1Velocity = vel;
RobertoO 0:67c50348f842 89
paulstuiver 6:ea3b3f50c893 90 motor1Direction = (motorvalue > 0.0f); //boolean output: true gives counterclockwise direction, false gives clockwise direction
paulstuiver 2:75b2f713161c 91 }
RobertoO 0:67c50348f842 92
paulstuiver 2:75b2f713161c 93 // function to call reference velocity, measured velocity and controls motor with feedback
paulstuiver 2:75b2f713161c 94 void measureandcontrol(void)
paulstuiver 2:75b2f713161c 95 {
paulstuiver 6:ea3b3f50c893 96 float referenceposition = getreferenceposition();
paulstuiver 6:ea3b3f50c893 97 measuredposition = getmeasuredposition();
paulstuiver 6:ea3b3f50c893 98 motorvalue = PID_controller(referenceposition - measuredposition);
paulstuiver 6:ea3b3f50c893 99 sendtomotor(motorvalue);
paulstuiver 2:75b2f713161c 100 }
RobertoO 0:67c50348f842 101 int main()
RobertoO 0:67c50348f842 102 {
RobertoO 0:67c50348f842 103 pc.baud(115200);
paulstuiver 2:75b2f713161c 104 pc.printf("Starting...\r\n");
paulstuiver 2:75b2f713161c 105 motor1Velocity.period(period_signal);
paulstuiver 6:ea3b3f50c893 106 motor.attach(measureandcontrol, timeinterval);
paulstuiver 2:75b2f713161c 107 while(true)
paulstuiver 2:75b2f713161c 108 {
paulstuiver 2:75b2f713161c 109 wait(0.5);
paulstuiver 5:2ae500da8fe1 110 //pc.printf("velocity = %f\r\n", vel);
paulstuiver 5:2ae500da8fe1 111 //pc.printf("motor1Direction = %i\r\n", (int)motor1Direction);
paulstuiver 5:2ae500da8fe1 112 //pc.printf("motorvalue2 = %f\r\n", motorvalue2);
paulstuiver 5:2ae500da8fe1 113 pc.printf("number of counts %i\r\n", counts);
paulstuiver 6:ea3b3f50c893 114 pc.printf("measured position is %f\r\n", measuredposition);
paulstuiver 6:ea3b3f50c893 115 //pc.printf("position1 = %f\r\n",position11);
paulstuiver 6:ea3b3f50c893 116 //pc.printf("position2 = %f\r\n",position22);
paulstuiver 6:ea3b3f50c893 117 pc.printf("motorvalue is %f\r\n", motorvalue);
paulstuiver 6:ea3b3f50c893 118
paulstuiver 6:ea3b3f50c893 119 char c = pc.getcNb();
paulstuiver 6:ea3b3f50c893 120
paulstuiver 6:ea3b3f50c893 121 if (c == 'c')
paulstuiver 6:ea3b3f50c893 122 {
paulstuiver 6:ea3b3f50c893 123 pc.printf("Programm stops running");
paulstuiver 6:ea3b3f50c893 124 motorvalue = 0;
paulstuiver 6:ea3b3f50c893 125 sendtomotor(motorvalue);
paulstuiver 6:ea3b3f50c893 126 return -1;
paulstuiver 6:ea3b3f50c893 127 }
RobertoO 0:67c50348f842 128 }
paulstuiver 2:75b2f713161c 129 }