Starting Interface for CTS Control
Dependencies: mbed mbedWSEsbc
main.cpp@0:4dbf26d91614, 2021-02-09 (annotated)
- Committer:
- jdawkins
- Date:
- Tue Feb 09 18:15:06 2021 +0000
- Revision:
- 0:4dbf26d91614
CTS matlab code;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jdawkins | 0:4dbf26d91614 | 1 | #include "mbed.h" |
jdawkins | 0:4dbf26d91614 | 2 | #include "mbedWSEsbc.h" |
jdawkins | 0:4dbf26d91614 | 3 | |
jdawkins | 0:4dbf26d91614 | 4 | #define PI 3.14159 |
jdawkins | 0:4dbf26d91614 | 5 | #define LOG_FREQ 5.0 |
jdawkins | 0:4dbf26d91614 | 6 | #define CTRL_FREQ 100.0 |
jdawkins | 0:4dbf26d91614 | 7 | |
jdawkins | 0:4dbf26d91614 | 8 | |
jdawkins | 0:4dbf26d91614 | 9 | DigitalOut myled(LED1); |
jdawkins | 0:4dbf26d91614 | 10 | DigitalOut rx_LED(LED2); |
jdawkins | 0:4dbf26d91614 | 11 | DigitalOut log_LED(LED3); |
jdawkins | 0:4dbf26d91614 | 12 | DigitalOut run_LED(LED4); |
jdawkins | 0:4dbf26d91614 | 13 | |
jdawkins | 0:4dbf26d91614 | 14 | float pwm; |
jdawkins | 0:4dbf26d91614 | 15 | bool run_flag; |
jdawkins | 0:4dbf26d91614 | 16 | float dt; |
jdawkins | 0:4dbf26d91614 | 17 | char buf[64]; |
jdawkins | 0:4dbf26d91614 | 18 | Ticker control_tick; |
jdawkins | 0:4dbf26d91614 | 19 | Ticker log_tick; |
jdawkins | 0:4dbf26d91614 | 20 | float tank1,tank2; |
jdawkins | 0:4dbf26d91614 | 21 | |
jdawkins | 0:4dbf26d91614 | 22 | int parseMessage(char * msg) |
jdawkins | 0:4dbf26d91614 | 23 | { |
jdawkins | 0:4dbf26d91614 | 24 | if(!strncmp(msg, "$CMD", 4)) { |
jdawkins | 0:4dbf26d91614 | 25 | float arg1; |
jdawkins | 0:4dbf26d91614 | 26 | |
jdawkins | 0:4dbf26d91614 | 27 | sscanf(msg,"$CMD,%f\n",&arg1); |
jdawkins | 0:4dbf26d91614 | 28 | pwm = arg1; |
jdawkins | 0:4dbf26d91614 | 29 | // rx_LED = ! rx_LED;// toggle led to indicate position message receipt |
jdawkins | 0:4dbf26d91614 | 30 | run_flag = true; |
jdawkins | 0:4dbf26d91614 | 31 | |
jdawkins | 0:4dbf26d91614 | 32 | return 1; |
jdawkins | 0:4dbf26d91614 | 33 | } //end of $CMD |
jdawkins | 0:4dbf26d91614 | 34 | |
jdawkins | 0:4dbf26d91614 | 35 | if(!strncmp(msg, "$STP", 4)) { |
jdawkins | 0:4dbf26d91614 | 36 | rx_LED = ! rx_LED;// toggle led to indicate position message receipt |
jdawkins | 0:4dbf26d91614 | 37 | run_flag = false; |
jdawkins | 0:4dbf26d91614 | 38 | pwm = 0; |
jdawkins | 0:4dbf26d91614 | 39 | return 1; |
jdawkins | 0:4dbf26d91614 | 40 | } //end of $STP |
jdawkins | 0:4dbf26d91614 | 41 | |
jdawkins | 0:4dbf26d91614 | 42 | return 0; |
jdawkins | 0:4dbf26d91614 | 43 | } |
jdawkins | 0:4dbf26d91614 | 44 | |
jdawkins | 0:4dbf26d91614 | 45 | void controlLoop(){ |
jdawkins | 0:4dbf26d91614 | 46 | |
jdawkins | 0:4dbf26d91614 | 47 | tank2 = read_max1270_volts(0, 1, 1); //chan, range, bipol |
jdawkins | 0:4dbf26d91614 | 48 | tank1 = read_max1270_volts(1, 1, 1); |
jdawkins | 0:4dbf26d91614 | 49 | //Logic to exit loop if tank gets too full |
jdawkins | 0:4dbf26d91614 | 50 | if(tank1 > 4.1 || tank2 > 4.1) { |
jdawkins | 0:4dbf26d91614 | 51 | run_flag = false; |
jdawkins | 0:4dbf26d91614 | 52 | } |
jdawkins | 0:4dbf26d91614 | 53 | |
jdawkins | 0:4dbf26d91614 | 54 | if(run_flag) { |
jdawkins | 0:4dbf26d91614 | 55 | mot_control(1, -pwm); // negative value required due do polarity of pump motor |
jdawkins | 0:4dbf26d91614 | 56 | |
jdawkins | 0:4dbf26d91614 | 57 | } else { |
jdawkins | 0:4dbf26d91614 | 58 | mot_control(1,0); |
jdawkins | 0:4dbf26d91614 | 59 | } |
jdawkins | 0:4dbf26d91614 | 60 | myled = !myled; |
jdawkins | 0:4dbf26d91614 | 61 | } |
jdawkins | 0:4dbf26d91614 | 62 | |
jdawkins | 0:4dbf26d91614 | 63 | void logLoop(){ |
jdawkins | 0:4dbf26d91614 | 64 | |
jdawkins | 0:4dbf26d91614 | 65 | pc.printf("$SEN,%.3f,%.3f,%.3f\r\n", tank1,tank2,pwm); //write data to scren |
jdawkins | 0:4dbf26d91614 | 66 | |
jdawkins | 0:4dbf26d91614 | 67 | log_LED = !log_LED; |
jdawkins | 0:4dbf26d91614 | 68 | } |
jdawkins | 0:4dbf26d91614 | 69 | |
jdawkins | 0:4dbf26d91614 | 70 | int main() |
jdawkins | 0:4dbf26d91614 | 71 | { |
jdawkins | 0:4dbf26d91614 | 72 | pc.printf("Quanser Twin Tank Control Data Aquisition Program\r\n"); |
jdawkins | 0:4dbf26d91614 | 73 | |
jdawkins | 0:4dbf26d91614 | 74 | float log_timer = 0; //initialize variable for log timer |
jdawkins | 0:4dbf26d91614 | 75 | run_flag = true; |
jdawkins | 0:4dbf26d91614 | 76 | myled = 1; |
jdawkins | 0:4dbf26d91614 | 77 | pwm = 0; |
jdawkins | 0:4dbf26d91614 | 78 | // pc.attach(&rxCallback); |
jdawkins | 0:4dbf26d91614 | 79 | mbedWSEsbcInit(9600); //Initialize the mbed WSE Project SBC |
jdawkins | 0:4dbf26d91614 | 80 | wait(.2); //delay at beginning for voltage settle purposes |
jdawkins | 0:4dbf26d91614 | 81 | t.start(); // Set up timer |
jdawkins | 0:4dbf26d91614 | 82 | Timer tc; |
jdawkins | 0:4dbf26d91614 | 83 | tc.start(); |
jdawkins | 0:4dbf26d91614 | 84 | tc.reset(); |
jdawkins | 0:4dbf26d91614 | 85 | |
jdawkins | 0:4dbf26d91614 | 86 | control_tick.attach(&controlLoop, 0.1); |
jdawkins | 0:4dbf26d91614 | 87 | log_tick.attach(&logLoop,1.0); |
jdawkins | 0:4dbf26d91614 | 88 | |
jdawkins | 0:4dbf26d91614 | 89 | t.reset(); // zero timer |
jdawkins | 0:4dbf26d91614 | 90 | dt = 1/(CTRL_FREQ); |
jdawkins | 0:4dbf26d91614 | 91 | |
jdawkins | 0:4dbf26d91614 | 92 | while(1) { |
jdawkins | 0:4dbf26d91614 | 93 | |
jdawkins | 0:4dbf26d91614 | 94 | if(pc.readable()) { |
jdawkins | 0:4dbf26d91614 | 95 | int i=0; |
jdawkins | 0:4dbf26d91614 | 96 | char c = pc.getc(); |
jdawkins | 0:4dbf26d91614 | 97 | // pc.printf("%s",c); |
jdawkins | 0:4dbf26d91614 | 98 | if(c=='$') { |
jdawkins | 0:4dbf26d91614 | 99 | buf[i] = c; |
jdawkins | 0:4dbf26d91614 | 100 | i++; |
jdawkins | 0:4dbf26d91614 | 101 | while(1) { |
jdawkins | 0:4dbf26d91614 | 102 | buf[i] = pc.getc(); |
jdawkins | 0:4dbf26d91614 | 103 | if(buf[i]=='\n') { |
jdawkins | 0:4dbf26d91614 | 104 | break; |
jdawkins | 0:4dbf26d91614 | 105 | } |
jdawkins | 0:4dbf26d91614 | 106 | i++; |
jdawkins | 0:4dbf26d91614 | 107 | } |
jdawkins | 0:4dbf26d91614 | 108 | } |
jdawkins | 0:4dbf26d91614 | 109 | |
jdawkins | 0:4dbf26d91614 | 110 | parseMessage(buf); |
jdawkins | 0:4dbf26d91614 | 111 | |
jdawkins | 0:4dbf26d91614 | 112 | } // If PC Readable |
jdawkins | 0:4dbf26d91614 | 113 | |
jdawkins | 0:4dbf26d91614 | 114 | |
jdawkins | 0:4dbf26d91614 | 115 | //if run flag |
jdawkins | 0:4dbf26d91614 | 116 | wait(dt); //Wait for sample time of loop frequency |
jdawkins | 0:4dbf26d91614 | 117 | }//while |
jdawkins | 0:4dbf26d91614 | 118 | }//main |