Zhixin Li / Mbed 2 deprecated PES_L5Ex1a

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

main.cpp@0:0b04f8ba31ca, 2018-10-19 (annotated)

Committer:
Rhein
Date:
Fri Oct 19 11:44:56 2018 +0000
Revision:
0:0b04f8ba31ca
Child:
1:693bacef296b
PES_L5_Ex1a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Rhein 0:0b04f8ba31ca 1 #include "mbed.h"
Rhein 0:0b04f8ba31ca 2 #include "QEI.h"
Rhein 0:0b04f8ba31ca 3 #include "MODSERIAL.h"
Rhein 0:0b04f8ba31ca 4
Rhein 0:0b04f8ba31ca 5 DigitalOut gpo(D0);
Rhein 0:0b04f8ba31ca 6 DigitalOut led(LED_RED);
Rhein 0:0b04f8ba31ca 7 DigitalOut motor_dir(D7);
Rhein 0:0b04f8ba31ca 8
Rhein 0:0b04f8ba31ca 9 AnalogIn pot1(A2);
Rhein 0:0b04f8ba31ca 10 AnalogIn pot2(A3);
Rhein 0:0b04f8ba31ca 11
Rhein 0:0b04f8ba31ca 12 double Ts = 0.01; //Sample time in seconds
Rhein 0:0b04f8ba31ca 13 Serial pc(USBTX,USBRX);
Rhein 0:0b04f8ba31ca 14 pc.baud(115200);
Rhein 0:0b04f8ba31ca 15 pi = 3.14159261;
Rhein 0:0b04f8ba31ca 16 QEI Encoder(D12,D13,NC,32);
Rhein 0:0b04f8ba31ca 17 double countsrotation = 32;
Rhein 0:0b04f8ba31ca 18 double gear_ration = 131;
Rhein 0:0b04f8ba31ca 19 int main()
Rhein 0:0b04f8ba31ca 20 {
Rhein 0:0b04f8ba31ca 21
Rhein 0:0b04f8ba31ca 22 //Determine reference position in radians between 0 and 2pi
Rhein 0:0b04f8ba31ca 23
Rhein 0:0b04f8ba31ca 24 double pos_ref = pot1 * 2pi;
Rhein 0:0b04f8ba31ca 25 pc.printf("%f \r\n",pos_ref);
Rhein 0:0b04f8ba31ca 26
Rhein 0:0b04f8ba31ca 27 // get actual position(measure plant output)
Rhein 0:0b04f8ba31ca 28
Rhein 0:0b04f8ba31ca 29 int counts;
Rhein 0:0b04f8ba31ca 30
Rhein 0:0b04f8ba31ca 31 pos_counts = Encoder.getPulses();
Rhein 0:0b04f8ba31ca 32 double pos_offset_counts = 0;
Rhein 0:0b04f8ba31ca 33 double counts_per_rad = countsrotation / gear_ratio;
Rhein 0:0b04f8ba31ca 34
Rhein 0:0b04f8ba31ca 35 double pos_rad = - (pos_counts - pos_offset_counts) / counts_per_rad;
Rhein 0:0b04f8ba31ca 36
Rhein 0:0b04f8ba31ca 37 // P controller
Rhein 0:0b04f8ba31ca 38
Rhein 0:0b04f8ba31ca 39 double e = pos_ref - pos_rad;
Rhein 0:0b04f8ba31ca 40
Rhein 0:0b04f8ba31ca 41 double Kp = potmeter2.read();
Rhein 0:0b04f8ba31ca 42 double u_k = Kp * e;
Rhein 0:0b04f8ba31ca 43
Rhein 0:0b04f8ba31ca 44 }
Rhein 0:0b04f8ba31ca 45