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@5:2ae500da8fe1, 2019-10-07 (annotated)

Committer:
paulstuiver
Date:
Mon Oct 07 14:22:05 2019 +0000
Revision:
5:2ae500da8fe1
Parent:
3:4f281c336a8b
Child:
6:ea3b3f50c893
Child:
8:7dab565a208e
measured velocity, problems in position1 and position2

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 2:75b2f713161c 6
paulstuiver 2:75b2f713161c 7 DigitalIn button1(D12);
paulstuiver 2:75b2f713161c 8 AnalogIn pot2(A0);
paulstuiver 2:75b2f713161c 9 Ticker john;
paulstuiver 3:4f281c336a8b 10 DigitalOut motor1Direction(D7);
paulstuiver 3:4f281c336a8b 11 FastPWM motor1Velocity(D6);
paulstuiver 2:75b2f713161c 12 Serial pc(USBTX, USBRX);
paulstuiver 5:2ae500da8fe1 13 QEI Encoder(D8,D9,NC,8400);
paulstuiver 2:75b2f713161c 14 volatile double frequency = 17000.0;
paulstuiver 2:75b2f713161c 15 volatile double period_signal = 1.0/frequency;
paulstuiver 3:4f281c336a8b 16 float vel = 0.0f;
paulstuiver 3:4f281c336a8b 17 float referencevelocity;
paulstuiver 3:4f281c336a8b 18 float motorvalue2;
paulstuiver 5:2ae500da8fe1 19 double Kp = 17.5;
paulstuiver 5:2ae500da8fe1 20 int counts;
paulstuiver 5:2ae500da8fe1 21 float position1 = 0;
paulstuiver 5:2ae500da8fe1 22 float position2;
paulstuiver 5:2ae500da8fe1 23 float timeinterval = 0.001;
paulstuiver 5:2ae500da8fe1 24 float measuredvelocity;
paulstuiver 5:2ae500da8fe1 25
paulstuiver 5:2ae500da8fe1 26 // README
paulstuiver 5:2ae500da8fe1 27 // positive referencevelocity corresponds to a counterclockwise motion
paulstuiver 5:2ae500da8fe1 28
paulstuiver 5:2ae500da8fe1 29
paulstuiver 5:2ae500da8fe1 30 //P control implementation (behaves like a spring)
paulstuiver 5:2ae500da8fe1 31 double P_controller(double error)
paulstuiver 5:2ae500da8fe1 32 {
paulstuiver 5:2ae500da8fe1 33 //Proportional part:
paulstuiver 5:2ae500da8fe1 34 double u_k = Kp * error;
paulstuiver 5:2ae500da8fe1 35
paulstuiver 5:2ae500da8fe1 36 //sum all parts and return it
paulstuiver 5:2ae500da8fe1 37 return u_k;
paulstuiver 5:2ae500da8fe1 38 }
paulstuiver 2:75b2f713161c 39
paulstuiver 2:75b2f713161c 40 //get the measured velocity
paulstuiver 2:75b2f713161c 41 double getmeasuredvelocity()
paulstuiver 2:75b2f713161c 42 {
paulstuiver 5:2ae500da8fe1 43 counts = Encoder.getPulses();
paulstuiver 5:2ae500da8fe1 44 counts = counts % 8400;
paulstuiver 5:2ae500da8fe1 45 position2 = counts/8400*2*3.141592;
paulstuiver 5:2ae500da8fe1 46 measuredvelocity = (position2-position1)/timeinterval;
paulstuiver 5:2ae500da8fe1 47 position1 = position2;
paulstuiver 2:75b2f713161c 48 }
paulstuiver 2:75b2f713161c 49
paulstuiver 2:75b2f713161c 50 //get the reference of the velocity: positive or negative
paulstuiver 2:75b2f713161c 51 double getreferencevelocity()
paulstuiver 2:75b2f713161c 52 {
paulstuiver 3:4f281c336a8b 53 referencevelocity = -1.0 + 2.0*pot2.read();
paulstuiver 2:75b2f713161c 54 return referencevelocity;
paulstuiver 2:75b2f713161c 55 }
RobertoO 0:67c50348f842 56
paulstuiver 2:75b2f713161c 57 //send value to motor
paulstuiver 2:75b2f713161c 58 void sendtomotor(float motorvalue)
paulstuiver 2:75b2f713161c 59 {
paulstuiver 3:4f281c336a8b 60 motorvalue2 = motorvalue;
paulstuiver 2:75b2f713161c 61 vel = fabs(motorvalue);
paulstuiver 3:4f281c336a8b 62 vel = vel > 1.0f ? 1.0f : vel;
paulstuiver 2:75b2f713161c 63 motor1Velocity = vel;
RobertoO 0:67c50348f842 64
paulstuiver 2:75b2f713161c 65 motor1Direction = (motorvalue > 0.0f);
paulstuiver 2:75b2f713161c 66 }
RobertoO 0:67c50348f842 67
paulstuiver 2:75b2f713161c 68 // function to call reference velocity, measured velocity and controls motor with feedback
paulstuiver 2:75b2f713161c 69 void measureandcontrol(void)
paulstuiver 2:75b2f713161c 70 {
paulstuiver 2:75b2f713161c 71 float referencevelocity = getreferencevelocity();
paulstuiver 2:75b2f713161c 72 float measuredvelocity = getmeasuredvelocity();
paulstuiver 2:75b2f713161c 73 sendtomotor(referencevelocity);
paulstuiver 2:75b2f713161c 74 }
RobertoO 0:67c50348f842 75 int main()
RobertoO 0:67c50348f842 76 {
RobertoO 0:67c50348f842 77 pc.baud(115200);
paulstuiver 2:75b2f713161c 78 pc.printf("Starting...\r\n");
paulstuiver 2:75b2f713161c 79 motor1Velocity.period(period_signal);
paulstuiver 5:2ae500da8fe1 80 john.attach(measureandcontrol, timeinterval);
paulstuiver 2:75b2f713161c 81 while(true)
paulstuiver 2:75b2f713161c 82 {
paulstuiver 2:75b2f713161c 83 wait(0.5);
paulstuiver 5:2ae500da8fe1 84 //pc.printf("velocity = %f\r\n", vel);
paulstuiver 5:2ae500da8fe1 85 //pc.printf("motor1Direction = %i\r\n", (int)motor1Direction);
paulstuiver 5:2ae500da8fe1 86 //pc.printf("motorvalue2 = %f\r\n", motorvalue2);
paulstuiver 5:2ae500da8fe1 87 pc.printf("number of counts %i\r\n", counts);
paulstuiver 5:2ae500da8fe1 88 pc.printf("measured velocity is %f\r\n", measuredvelocity);
paulstuiver 5:2ae500da8fe1 89 pc.printf("position1 = %f\r\n",position1);
paulstuiver 5:2ae500da8fe1 90 pc.printf("position2 = %f\r\n",position2);
RobertoO 0:67c50348f842 91 }
paulstuiver 2:75b2f713161c 92 }