Luke Ledva
/
LogicControl
Commented Logic Control
main.cpp@0:70e2da623aa5, 2017-03-12 (annotated)
- Committer:
- m193672
- Date:
- Sun Mar 12 17:56:36 2017 +0000
- Revision:
- 0:70e2da623aa5
LogicControl
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
m193672 | 0:70e2da623aa5 | 1 | /************************************************************************************* |
m193672 | 0:70e2da623aa5 | 2 | Program Name: ES202 Laboratory Experiment 4, week 1 |
m193672 | 0:70e2da623aa5 | 3 | |
m193672 | 0:70e2da623aa5 | 4 | Description: |
m193672 | 0:70e2da623aa5 | 5 | This program takes two values (the number of measurements to collect out and the motor |
m193672 | 0:70e2da623aa5 | 6 | pwm) from a user. The program then collects the prescribed number measurements |
m193672 | 0:70e2da623aa5 | 7 | and sends the iteration stamped values across the serial port. |
m193672 | 0:70e2da623aa5 | 8 | NOTE: YOU MUST CHANGE THE PWM PERIOD IN THE MOTOR.CPP FILE! CHANGE TO _pwm.period_us(50); |
m193672 | 0:70e2da623aa5 | 9 | Author: Levi DeVries, PhD, USNA, 2/1/17 |
m193672 | 0:70e2da623aa5 | 10 | **************************************************************************************/ |
m193672 | 0:70e2da623aa5 | 11 | |
m193672 | 0:70e2da623aa5 | 12 | #include "mbed.h" |
m193672 | 0:70e2da623aa5 | 13 | #include "Motor.h" |
m193672 | 0:70e2da623aa5 | 14 | |
m193672 | 0:70e2da623aa5 | 15 | Serial pc(USBTX,USBRX); // serial comms with PC |
m193672 | 0:70e2da623aa5 | 16 | |
m193672 | 0:70e2da623aa5 | 17 | // ENTER CORRECT PIN NUMBER HERE |
m193672 | 0:70e2da623aa5 | 18 | AnalogIn sensor(p20); // analog sensor |
m193672 | 0:70e2da623aa5 | 19 | |
m193672 | 0:70e2da623aa5 | 20 | // ENTER CORRECT MOTOR PINS HERE |
m193672 | 0:70e2da623aa5 | 21 | Motor mot(p26,p30,p29); // motor object |
m193672 | 0:70e2da623aa5 | 22 | |
m193672 | 0:70e2da623aa5 | 23 | int counter; // counter variable to print across serial port |
m193672 | 0:70e2da623aa5 | 24 | int Ncnts = 0; // number of iterations to count to |
m193672 | 0:70e2da623aa5 | 25 | float aval = 0.0; // analog measurement value |
m193672 | 0:70e2da623aa5 | 26 | float mDC = 0.0; // motor duty cycle |
m193672 | 0:70e2da623aa5 | 27 | float dheight=0.0; |
m193672 | 0:70e2da623aa5 | 28 | float meas_height; |
m193672 | 0:70e2da623aa5 | 29 | float height_error; |
m193672 | 0:70e2da623aa5 | 30 | int main() |
m193672 | 0:70e2da623aa5 | 31 | { |
m193672 | 0:70e2da623aa5 | 32 | pc.baud(115200); // baud rate |
m193672 | 0:70e2da623aa5 | 33 | |
m193672 | 0:70e2da623aa5 | 34 | while(1) { |
m193672 | 0:70e2da623aa5 | 35 | mot.speed(0.0); // turn motor off at start of program |
m193672 | 0:70e2da623aa5 | 36 | |
m193672 | 0:70e2da623aa5 | 37 | pc.scanf("%d,%f",&Ncnts,&dheight); |
m193672 | 0:70e2da623aa5 | 38 | // ENTER CODE HERE TO READ NUMBER OF COUNTS AND DUTY CYCLE FROM SERIAL PORT!!! |
m193672 | 0:70e2da623aa5 | 39 | |
m193672 | 0:70e2da623aa5 | 40 | |
m193672 | 0:70e2da623aa5 | 41 | counter = 1; // set counter to 1 for new experiment |
m193672 | 0:70e2da623aa5 | 42 | while(counter<=Ncnts) { |
m193672 | 0:70e2da623aa5 | 43 | |
m193672 | 0:70e2da623aa5 | 44 | aval=sensor.read();// Read from sensor |
m193672 | 0:70e2da623aa5 | 45 | meas_height = 568.3*pow(aval,3) - 797.8*pow(aval,2) + 381*pow(aval,1) - 41; //polyfit funciton coefficients used to accurately model the height |
m193672 | 0:70e2da623aa5 | 46 | height_error = dheight - meas_height; //determine the distance between the desired height and actual height |
m193672 | 0:70e2da623aa5 | 47 | // logic control |
m193672 | 0:70e2da623aa5 | 48 | if(height_error > 1.0) { //when the elevator is greater than 1 inch below the desired height |
m193672 | 0:70e2da623aa5 | 49 | mDC = 1.0; //drive the elevator up |
m193672 | 0:70e2da623aa5 | 50 | } else if(height_error < -1.0) { //when the elevator is greater than one inch above the desired height |
m193672 | 0:70e2da623aa5 | 51 | mDC = -1.0; //drive the elevator down |
m193672 | 0:70e2da623aa5 | 52 | } else { //if the elevator is between 1 inch above and 1 inch below the desired height |
m193672 | 0:70e2da623aa5 | 53 | mDC = 0.0; // stop the elevator by turning off the DC Motor |
m193672 | 0:70e2da623aa5 | 54 | } |
m193672 | 0:70e2da623aa5 | 55 | |
m193672 | 0:70e2da623aa5 | 56 | pc.printf("%d,%f,%f\n",counter,meas_height,mDC); |
m193672 | 0:70e2da623aa5 | 57 | mot.speed(mDC);// ENTER CODE HERE TO SET MOTOR DUTY CYCLE |
m193672 | 0:70e2da623aa5 | 58 | |
m193672 | 0:70e2da623aa5 | 59 | //pc.printf("%d,%f,%f\n",counter,aval,mDC);// PRINT ITERATION #, SENSOR MEASUREMENT, AND DUTY CYCLE TO SERIAL PORT |
m193672 | 0:70e2da623aa5 | 60 | |
m193672 | 0:70e2da623aa5 | 61 | |
m193672 | 0:70e2da623aa5 | 62 | |
m193672 | 0:70e2da623aa5 | 63 | |
m193672 | 0:70e2da623aa5 | 64 | counter++; // update counter |
m193672 | 0:70e2da623aa5 | 65 | wait(0.05); // wait 0.05 seconds to allow sensor to refresh |
m193672 | 0:70e2da623aa5 | 66 | |
m193672 | 0:70e2da623aa5 | 67 | } // end while(counter) |
m193672 | 0:70e2da623aa5 | 68 | }// end while(1) |
m193672 | 0:70e2da623aa5 | 69 | } // end main |