James Morris / Mbed 2 deprecated ELCT302_Lab3_Testing

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:bd171e51f222, 2021-03-08 (annotated)

Committer:
jamescmorrisiv
Date:
Mon Mar 08 23:08:25 2021 +0000
Revision:
0:bd171e51f222
Child:
1:01aa18e7c4e5
End of Day 3/8/2021

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jamescmorrisiv 0:bd171e51f222 1 #include "mbed.h"
jamescmorrisiv 0:bd171e51f222 2 #include <string>
jamescmorrisiv 0:bd171e51f222 3 #include <sstream>
jamescmorrisiv 0:bd171e51f222 4
jamescmorrisiv 0:bd171e51f222 5 using namespace std;
jamescmorrisiv 0:bd171e51f222 6
jamescmorrisiv 0:bd171e51f222 7 Serial pc(USBTX,USBRX);
jamescmorrisiv 0:bd171e51f222 8 AnalogIn wheelAngleControl(PTB0);
jamescmorrisiv 0:bd171e51f222 9 AnalogIn leftWheelDSense(PTB1);
jamescmorrisiv 0:bd171e51f222 10 AnalogIn rightWheelDSense(PTB2);
jamescmorrisiv 0:bd171e51f222 11 PwmOut pwmWheelAngle(PTE20);
jamescmorrisiv 0:bd171e51f222 12 PwmOut BlueLED(LED3);
jamescmorrisiv 0:bd171e51f222 13
jamescmorrisiv 0:bd171e51f222 14 //Control System Variables
jamescmorrisiv 0:bd171e51f222 15 #define TI 0.001 //1kHz Sample Time
jamescmorrisiv 0:bd171e51f222 16 #define KP 0.2078f //Proportional Gain
jamescmorrisiv 0:bd171e51f222 17 #define KD 1.6231f //Integral Gain
jamescmorrisiv 0:bd171e51f222 18
jamescmorrisiv 0:bd171e51f222 19 //All inital values and variables declared
jamescmorrisiv 0:bd171e51f222 20 float max=0.1f;
jamescmorrisiv 0:bd171e51f222 21 float min=0.05f;
jamescmorrisiv 0:bd171e51f222 22 float mid;
jamescmorrisiv 0:bd171e51f222 23 float range;
jamescmorrisiv 0:bd171e51f222 24 float currVal=0.0f;
jamescmorrisiv 0:bd171e51f222 25 float leftDist=0.5f;
jamescmorrisiv 0:bd171e51f222 26 float rightDist=0.5f;
jamescmorrisiv 0:bd171e51f222 27 float leftDat=0.0f;
jamescmorrisiv 0:bd171e51f222 28 float rightDat=0.0f;
jamescmorrisiv 0:bd171e51f222 29 float average;
jamescmorrisiv 0:bd171e51f222 30
jamescmorrisiv 0:bd171e51f222 31 float errorSignal;
jamescmorrisiv 0:bd171e51f222 32 float errorLast;
jamescmorrisiv 0:bd171e51f222 33
jamescmorrisiv 0:bd171e51f222 34 int state=0; // 0=off 1=on 2=wait;
jamescmorrisiv 0:bd171e51f222 35
jamescmorrisiv 0:bd171e51f222 36 Ticker sampler;
jamescmorrisiv 0:bd171e51f222 37
jamescmorrisiv 0:bd171e51f222 38 void sample()
jamescmorrisiv 0:bd171e51f222 39 {
jamescmorrisiv 0:bd171e51f222 40 leftDist=leftWheelDSense.read();
jamescmorrisiv 0:bd171e51f222 41 rightDist=rightWheelDSense.read();
jamescmorrisiv 0:bd171e51f222 42 leftDat=(12.185*leftDist)-3.83;
jamescmorrisiv 0:bd171e51f222 43 rightDat=(-15.77*rightDist)+4.75;
jamescmorrisiv 0:bd171e51f222 44 average=(leftDat+rightDat)/2;
jamescmorrisiv 0:bd171e51f222 45 errorLast=errorSignal;
jamescmorrisiv 0:bd171e51f222 46 errorSignal=average;
jamescmorrisiv 0:bd171e51f222 47 return;
jamescmorrisiv 0:bd171e51f222 48 }
jamescmorrisiv 0:bd171e51f222 49
jamescmorrisiv 0:bd171e51f222 50 int main() {
jamescmorrisiv 0:bd171e51f222 51
jamescmorrisiv 0:bd171e51f222 52 //Control System Variables
jamescmorrisiv 0:bd171e51f222 53 # define TI 0.001 //1kHz Sample Time
jamescmorrisiv 0:bd171e51f222 54 # define KP 0.7299f //Proportional Gain
jamescmorrisiv 0:bd171e51f222 55 # define KD 5.1302f //Integral Gain
jamescmorrisiv 0:bd171e51f222 56 # define M_PI 3.14159265358979323846
jamescmorrisiv 0:bd171e51f222 57
jamescmorrisiv 0:bd171e51f222 58 //All inital values and variables declared
jamescmorrisiv 0:bd171e51f222 59 float max=0.1f;
jamescmorrisiv 0:bd171e51f222 60 float min=0.05f;
jamescmorrisiv 0:bd171e51f222 61 float mid=(max+min)/2;
jamescmorrisiv 0:bd171e51f222 62 float range=max-min;
jamescmorrisiv 0:bd171e51f222 63 pwmWheelAngle.period(0.02f);
jamescmorrisiv 0:bd171e51f222 64 float currVal=0.0f;
jamescmorrisiv 0:bd171e51f222 65 float leftDist=0.5f;
jamescmorrisiv 0:bd171e51f222 66 float rightDist=0.5f;
jamescmorrisiv 0:bd171e51f222 67 float leftDat=0.0f;
jamescmorrisiv 0:bd171e51f222 68 float rightDat=0.0f;
jamescmorrisiv 0:bd171e51f222 69 float duty=mid;
jamescmorrisiv 0:bd171e51f222 70 float reference=0;
jamescmorrisiv 0:bd171e51f222 71
jamescmorrisiv 0:bd171e51f222 72 float errorSignal = 0.0f;
jamescmorrisiv 0:bd171e51f222 73 float errorCurr = 0.0f;
jamescmorrisiv 0:bd171e51f222 74 float errorLast = 0.0f;
jamescmorrisiv 0:bd171e51f222 75 float errorChange = 0.0f;
jamescmorrisiv 0:bd171e51f222 76 sampler.attach(&sample,TI);
jamescmorrisiv 0:bd171e51f222 77 float controllerOutput=3.14/2;
jamescmorrisiv 0:bd171e51f222 78
jamescmorrisiv 0:bd171e51f222 79 int state=0; // 0=off 1=on 2=wait;
jamescmorrisiv 0:bd171e51f222 80
jamescmorrisiv 0:bd171e51f222 81 //LED cycles on and off to show proper response
jamescmorrisiv 0:bd171e51f222 82 BlueLED=0.9f;
jamescmorrisiv 0:bd171e51f222 83 wait(0.9);
jamescmorrisiv 0:bd171e51f222 84 BlueLED=1.0f;
jamescmorrisiv 0:bd171e51f222 85 wait(0.9);
jamescmorrisiv 0:bd171e51f222 86 BlueLED=0.9f;
jamescmorrisiv 0:bd171e51f222 87 wait(0.9);
jamescmorrisiv 0:bd171e51f222 88 BlueLED=1.0f;
jamescmorrisiv 0:bd171e51f222 89
jamescmorrisiv 0:bd171e51f222 90
jamescmorrisiv 0:bd171e51f222 91 state=1;
jamescmorrisiv 0:bd171e51f222 92
jamescmorrisiv 0:bd171e51f222 93
jamescmorrisiv 0:bd171e51f222 94 while(1) { //main loop that cycles forever
jamescmorrisiv 0:bd171e51f222 95
jamescmorrisiv 0:bd171e51f222 96 //State check
jamescmorrisiv 0:bd171e51f222 97 if(state==0){
jamescmorrisiv 0:bd171e51f222 98 return 0;
jamescmorrisiv 0:bd171e51f222 99 }
jamescmorrisiv 0:bd171e51f222 100 else if(state==2){
jamescmorrisiv 0:bd171e51f222 101 BlueLED=0.80f;
jamescmorrisiv 0:bd171e51f222 102 } else {
jamescmorrisiv 0:bd171e51f222 103 BlueLED=1.0f;
jamescmorrisiv 0:bd171e51f222 104 }
jamescmorrisiv 0:bd171e51f222 105
jamescmorrisiv 0:bd171e51f222 106 //PD Tranfer Function
jamescmorrisiv 0:bd171e51f222 107 errorChange=(errorSignal-errorLast)/TI; // NO REFERNCE DERIVATIVE, NOT SURE WHAT TO PUT HERE-0.036495
jamescmorrisiv 0:bd171e51f222 108 errorCurr=errorSignal-reference;
jamescmorrisiv 0:bd171e51f222 109 controllerOutput=KP*errorCurr+KD*errorChange;
jamescmorrisiv 0:bd171e51f222 110
jamescmorrisiv 0:bd171e51f222 111 //Output Value Derivation
jamescmorrisiv 0:bd171e51f222 112 currVal=(controllerOutput*-1)+M_PI;
jamescmorrisiv 0:bd171e51f222 113 duty=min+(currVal*range);
jamescmorrisiv 0:bd171e51f222 114 if(duty<min){
jamescmorrisiv 0:bd171e51f222 115 duty=min;
jamescmorrisiv 0:bd171e51f222 116 }
jamescmorrisiv 0:bd171e51f222 117 if(duty>max){
jamescmorrisiv 0:bd171e51f222 118 duty=max;
jamescmorrisiv 0:bd171e51f222 119 }
jamescmorrisiv 0:bd171e51f222 120
jamescmorrisiv 0:bd171e51f222 121
jamescmorrisiv 0:bd171e51f222 122 //Changes PWM signal to appropriate value (steering control)
jamescmorrisiv 0:bd171e51f222 123 if(state==1){
jamescmorrisiv 0:bd171e51f222 124 pwmWheelAngle.write(duty); //uses potentiometer value
jamescmorrisiv 0:bd171e51f222 125 }
jamescmorrisiv 0:bd171e51f222 126
jamescmorrisiv 0:bd171e51f222 127 wait(0.0001);
jamescmorrisiv 0:bd171e51f222 128 }
jamescmorrisiv 0:bd171e51f222 129 }