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@1:01aa18e7c4e5, 2021-03-15 (annotated)

Committer:
jamescmorrisiv
Date:
Mon Mar 15 22:12:10 2021 +0000
Revision:
1:01aa18e7c4e5
Parent:
0:bd171e51f222 
End of day 3/15/2021 Before Check Off

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 1:01aa18e7c4e5 3 #include <vector>
jamescmorrisiv 1:01aa18e7c4e5 4 #include <iostream>
jamescmorrisiv 0:bd171e51f222 5
jamescmorrisiv 0:bd171e51f222 6 using namespace std;
jamescmorrisiv 0:bd171e51f222 7
jamescmorrisiv 0:bd171e51f222 8 Serial pc(USBTX,USBRX);
jamescmorrisiv 0:bd171e51f222 9 AnalogIn wheelAngleControl(PTB0);
jamescmorrisiv 0:bd171e51f222 10 AnalogIn leftWheelDSense(PTB1);
jamescmorrisiv 0:bd171e51f222 11 AnalogIn rightWheelDSense(PTB2);
jamescmorrisiv 0:bd171e51f222 12 PwmOut pwmWheelAngle(PTE20);
jamescmorrisiv 0:bd171e51f222 13 PwmOut BlueLED(LED3);
jamescmorrisiv 0:bd171e51f222 14
jamescmorrisiv 0:bd171e51f222 15 //Control System Variables
jamescmorrisiv 1:01aa18e7c4e5 16 #define TI 0.02 //50Hz Sample Time
jamescmorrisiv 1:01aa18e7c4e5 17 #define KP 2.6f //Proportional Gain 0.2
jamescmorrisiv 1:01aa18e7c4e5 18 #define KD 0.135f //Derivative Gain //1.6
jamescmorrisiv 1:01aa18e7c4e5 19 # define M_PI 3.14159265358979323846
jamescmorrisiv 0:bd171e51f222 20
jamescmorrisiv 0:bd171e51f222 21 //All inital values and variables declared
jamescmorrisiv 0:bd171e51f222 22 float max=0.1f;
jamescmorrisiv 0:bd171e51f222 23 float min=0.05f;
jamescmorrisiv 0:bd171e51f222 24 float mid;
jamescmorrisiv 0:bd171e51f222 25 float range;
jamescmorrisiv 0:bd171e51f222 26 float currVal=0.0f;
jamescmorrisiv 0:bd171e51f222 27 float leftDist=0.5f;
jamescmorrisiv 0:bd171e51f222 28 float rightDist=0.5f;
jamescmorrisiv 0:bd171e51f222 29 float leftDat=0.0f;
jamescmorrisiv 0:bd171e51f222 30 float rightDat=0.0f;
jamescmorrisiv 0:bd171e51f222 31 float average;
jamescmorrisiv 1:01aa18e7c4e5 32
jamescmorrisiv 1:01aa18e7c4e5 33 std::vector<float> V;
jamescmorrisiv 1:01aa18e7c4e5 34
jamescmorrisiv 0:bd171e51f222 35 float errorSignal;
jamescmorrisiv 0:bd171e51f222 36 float errorLast;
jamescmorrisiv 0:bd171e51f222 37
jamescmorrisiv 0:bd171e51f222 38 int state=0; // 0=off 1=on 2=wait;
jamescmorrisiv 0:bd171e51f222 39
jamescmorrisiv 0:bd171e51f222 40 Ticker sampler;
jamescmorrisiv 0:bd171e51f222 41
jamescmorrisiv 1:01aa18e7c4e5 42 void sample(void)
jamescmorrisiv 0:bd171e51f222 43 {
jamescmorrisiv 0:bd171e51f222 44 leftDist=leftWheelDSense.read();
jamescmorrisiv 0:bd171e51f222 45 rightDist=rightWheelDSense.read();
jamescmorrisiv 1:01aa18e7c4e5 46 average=(leftDist-rightDist);
jamescmorrisiv 0:bd171e51f222 47 errorLast=errorSignal;
jamescmorrisiv 1:01aa18e7c4e5 48 errorSignal=(13.421*average+3.1051)*(1.0f/100.0f);
jamescmorrisiv 1:01aa18e7c4e5 49 V[1]=V[0]; //Store the last value
jamescmorrisiv 1:01aa18e7c4e5 50 V[0]=errorSignal;
jamescmorrisiv 1:01aa18e7c4e5 51
jamescmorrisiv 1:01aa18e7c4e5 52 // pc.printf("Made it erhe %f\r\n\r\n",leftDist);
jamescmorrisiv 0:bd171e51f222 53 }
jamescmorrisiv 0:bd171e51f222 54
jamescmorrisiv 0:bd171e51f222 55 int main() {
jamescmorrisiv 0:bd171e51f222 56
jamescmorrisiv 0:bd171e51f222 57 //Control System Variables
jamescmorrisiv 1:01aa18e7c4e5 58
jamescmorrisiv 0:bd171e51f222 59
jamescmorrisiv 0:bd171e51f222 60 //All inital values and variables declared
jamescmorrisiv 0:bd171e51f222 61 float max=0.1f;
jamescmorrisiv 0:bd171e51f222 62 float min=0.05f;
jamescmorrisiv 0:bd171e51f222 63 float mid=(max+min)/2;
jamescmorrisiv 0:bd171e51f222 64 float range=max-min;
jamescmorrisiv 0:bd171e51f222 65 pwmWheelAngle.period(0.02f);
jamescmorrisiv 0:bd171e51f222 66 float currVal=0.0f;
jamescmorrisiv 0:bd171e51f222 67 float duty=mid;
jamescmorrisiv 0:bd171e51f222 68 float reference=0;
jamescmorrisiv 0:bd171e51f222 69
jamescmorrisiv 1:01aa18e7c4e5 70 V = std::vector<float>(2,0);
jamescmorrisiv 1:01aa18e7c4e5 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 1:01aa18e7c4e5 76 float controllerOutput=M_PI/2;
jamescmorrisiv 0:bd171e51f222 77
jamescmorrisiv 0:bd171e51f222 78 int state=0; // 0=off 1=on 2=wait;
jamescmorrisiv 0:bd171e51f222 79
jamescmorrisiv 0:bd171e51f222 80 //LED cycles on and off to show proper response
jamescmorrisiv 0:bd171e51f222 81 BlueLED=0.9f;
jamescmorrisiv 0:bd171e51f222 82 wait(0.9);
jamescmorrisiv 0:bd171e51f222 83 BlueLED=1.0f;
jamescmorrisiv 0:bd171e51f222 84 wait(0.9);
jamescmorrisiv 0:bd171e51f222 85 BlueLED=0.9f;
jamescmorrisiv 0:bd171e51f222 86 wait(0.9);
jamescmorrisiv 0:bd171e51f222 87 BlueLED=1.0f;
jamescmorrisiv 0:bd171e51f222 88
jamescmorrisiv 0:bd171e51f222 89
jamescmorrisiv 0:bd171e51f222 90 state=1;
jamescmorrisiv 0:bd171e51f222 91
jamescmorrisiv 1:01aa18e7c4e5 92 sampler.attach(sample,TI);
jamescmorrisiv 0:bd171e51f222 93 while(1) { //main loop that cycles forever
jamescmorrisiv 1:01aa18e7c4e5 94
jamescmorrisiv 1:01aa18e7c4e5 95 //Import Values
jamescmorrisiv 1:01aa18e7c4e5 96 errorSignal=V[0];
jamescmorrisiv 1:01aa18e7c4e5 97 errorLast=V[1];
jamescmorrisiv 0:bd171e51f222 98
jamescmorrisiv 0:bd171e51f222 99 //State check
jamescmorrisiv 0:bd171e51f222 100 if(state==0){
jamescmorrisiv 0:bd171e51f222 101 return 0;
jamescmorrisiv 0:bd171e51f222 102 }
jamescmorrisiv 0:bd171e51f222 103 else if(state==2){
jamescmorrisiv 0:bd171e51f222 104 BlueLED=0.80f;
jamescmorrisiv 0:bd171e51f222 105 } else {
jamescmorrisiv 0:bd171e51f222 106 BlueLED=1.0f;
jamescmorrisiv 0:bd171e51f222 107 }
jamescmorrisiv 0:bd171e51f222 108
jamescmorrisiv 0:bd171e51f222 109 //PD Tranfer Function
jamescmorrisiv 0:bd171e51f222 110 errorChange=(errorSignal-errorLast)/TI; // NO REFERNCE DERIVATIVE, NOT SURE WHAT TO PUT HERE-0.036495
jamescmorrisiv 0:bd171e51f222 111 errorCurr=errorSignal-reference;
jamescmorrisiv 0:bd171e51f222 112 controllerOutput=KP*errorCurr+KD*errorChange;
jamescmorrisiv 0:bd171e51f222 113
jamescmorrisiv 0:bd171e51f222 114 //Output Value Derivation
jamescmorrisiv 1:01aa18e7c4e5 115 currVal=(controllerOutput*-1)+(M_PI/2);
jamescmorrisiv 1:01aa18e7c4e5 116 duty=min+((currVal/M_PI)*range);
jamescmorrisiv 0:bd171e51f222 117 if(duty<min){
jamescmorrisiv 0:bd171e51f222 118 duty=min;
jamescmorrisiv 0:bd171e51f222 119 }
jamescmorrisiv 0:bd171e51f222 120 if(duty>max){
jamescmorrisiv 0:bd171e51f222 121 duty=max;
jamescmorrisiv 0:bd171e51f222 122 }
jamescmorrisiv 0:bd171e51f222 123
jamescmorrisiv 0:bd171e51f222 124
jamescmorrisiv 0:bd171e51f222 125 //Changes PWM signal to appropriate value (steering control)
jamescmorrisiv 0:bd171e51f222 126 if(state==1){
jamescmorrisiv 0:bd171e51f222 127 pwmWheelAngle.write(duty); //uses potentiometer value
jamescmorrisiv 1:01aa18e7c4e5 128 pc.printf("Duty at %f\r\n",duty);
jamescmorrisiv 1:01aa18e7c4e5 129 pc.printf("Curr Val at %f\r\n",currVal);
jamescmorrisiv 1:01aa18e7c4e5 130 pc.printf("Average is %f\r\n\r\n",V[0]*(100));
jamescmorrisiv 0:bd171e51f222 131 }
jamescmorrisiv 0:bd171e51f222 132
jamescmorrisiv 1:01aa18e7c4e5 133 wait(0.001);
jamescmorrisiv 0:bd171e51f222 134 }
jamescmorrisiv 0:bd171e51f222 135 }