Andy Wong / Mbed 2 deprecated elct302_lab3_PD_control_50hz

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:b781e625aca4, 2021-02-27 (annotated)

Committer:
awandy
Date:
Sat Feb 27 20:38:26 2021 +0000
Revision:
0:b781e625aca4
q

Who changed what in which revision?

UserRevisionLine numberNew contents of line
awandy 0:b781e625aca4 1 // reads input voltage from distance measurement circuit and outputs to console
awandy 0:b781e625aca4 2
awandy 0:b781e625aca4 3 #include "mbed.h"
awandy 0:b781e625aca4 4 #include <iostream>
awandy 0:b781e625aca4 5
awandy 0:b781e625aca4 6 //analog inputs to the microcontroller
awandy 0:b781e625aca4 7 AnalogIn Lin(PTB2);
awandy 0:b781e625aca4 8 AnalogIn Rin(PTB1);
awandy 0:b781e625aca4 9 //pwm output to servo
awandy 0:b781e625aca4 10 PwmOut output(PTB3);
awandy 0:b781e625aca4 11
awandy 0:b781e625aca4 12 // ticker to sample error
awandy 0:b781e625aca4 13 const float ti = 2e-2;
awandy 0:b781e625aca4 14 const float PI = 3.1415926535;
awandy 0:b781e625aca4 15 Ticker updater;
awandy 0:b781e625aca4 16
awandy 0:b781e625aca4 17 //initializing variables
awandy 0:b781e625aca4 18 float vL = 0;
awandy 0:b781e625aca4 19 float vR = 0;
awandy 0:b781e625aca4 20 float out = 0;
awandy 0:b781e625aca4 21 float turn_angle = 0;
awandy 0:b781e625aca4 22 float err = 0;
awandy 0:b781e625aca4 23 float prev_error = 0;
awandy 0:b781e625aca4 24 float del_error = 0;
awandy 0:b781e625aca4 25 int i = 0;
awandy 0:b781e625aca4 26 float sample_count = 20;
awandy 0:b781e625aca4 27
awandy 0:b781e625aca4 28 float sum = 0;
awandy 0:b781e625aca4 29
awandy 0:b781e625aca4 30 // turning constants
awandy 0:b781e625aca4 31 const float kp = 36.8;
awandy 0:b781e625aca4 32 const float kd = 2.128;
awandy 0:b781e625aca4 33 const float full_left = .059f; // duty cycle for full left turn
awandy 0:b781e625aca4 34 const float full_right = .091f; // duty cycle for full right turn
awandy 0:b781e625aca4 35 float d = full_left;
awandy 0:b781e625aca4 36
awandy 0:b781e625aca4 37 void pwmUpdate()
awandy 0:b781e625aca4 38 {
awandy 0:b781e625aca4 39 prev_error = err;
awandy 0:b781e625aca4 40 vL = Lin.read();
awandy 0:b781e625aca4 41 vR = Rin.read();
awandy 0:b781e625aca4 42
awandy 0:b781e625aca4 43 err = -(vL-vR) ; // voltage error
awandy 0:b781e625aca4 44 del_error = (err - prev_error)/ti; //change in voltage error
awandy 0:b781e625aca4 45 turn_angle = kp*err + kd*del_error; //calculates turn angle
awandy 0:b781e625aca4 46 if(turn_angle < 0)
awandy 0:b781e625aca4 47 turn_angle = 0;
awandy 0:b781e625aca4 48 if(turn_angle > PI)
awandy 0:b781e625aca4 49 turn_angle = PI;
awandy 0:b781e625aca4 50 d= (turn_angle/PI) * (full_right - full_left) + full_left;
awandy 0:b781e625aca4 51 if( abs(d - output.read()) > 0.01) //only updates the duty cycle if there is a change in the duty cycle greater than 0.3%
awandy 0:b781e625aca4 52 {
awandy 0:b781e625aca4 53 output.write(d);
awandy 0:b781e625aca4 54 }
awandy 0:b781e625aca4 55 cout << "del error: " << del_error << "\r" << std::endl;
awandy 0:b781e625aca4 56
awandy 0:b781e625aca4 57 }
awandy 0:b781e625aca4 58
awandy 0:b781e625aca4 59
awandy 0:b781e625aca4 60
awandy 0:b781e625aca4 61 int main(void)
awandy 0:b781e625aca4 62 {
awandy 0:b781e625aca4 63 output.period(0.02f); // 50 Hz control signal
awandy 0:b781e625aca4 64 // attaches pwmUpdate function to the ticker with a period of 20ms
awandy 0:b781e625aca4 65 updater.attach(&pwmUpdate,ti);
awandy 0:b781e625aca4 66
awandy 0:b781e625aca4 67 while(1) {} // do nothing
awandy 0:b781e625aca4 68 }