USNA WRC EW306 / Mbed 2 deprecated Motor_Control_SVFB

EW306 Motor Position Control with SVFB

Dependencies:   mbed mbedWSEsbc

main.cpp@1:d1a8fb968a8e, 2019-04-10 (annotated)

Committer:
jdawkins
Date:
Wed Apr 10 02:40:42 2019 +0000
Revision:
1:d1a8fb968a8e
Parent:
0:76487bb2de87
Child:
2:a5d160c21ec4
EW306 Motor Position Control Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 0:76487bb2de87 1 #include "mbed.h"
jdawkins 0:76487bb2de87 2 #include "mbedWSEsbc.h"
jdawkins 0:76487bb2de87 3
jdawkins 0:76487bb2de87 4 #define PI 3.14159
jdawkins 1:d1a8fb968a8e 5 #define LOG_FREQ 100.0
jdawkins 1:d1a8fb968a8e 6 #define CTRL_FREQ 100.0
jdawkins 0:76487bb2de87 7
jdawkins 0:76487bb2de87 8
jdawkins 0:76487bb2de87 9 DigitalOut myled(LED1);
jdawkins 1:d1a8fb968a8e 10 float tstop = 180; //initialize stop time
jdawkins 1:d1a8fb968a8e 11 float dc = 0; //initialize dc variable
jdawkins 1:d1a8fb968a8e 12 float dt = 1/CTRL_FREQ; // set control loop sample time
jdawkins 1:d1a8fb968a8e 13 float enc;
jdawkins 1:d1a8fb968a8e 14 float ang;
jdawkins 1:d1a8fb968a8e 15
jdawkins 0:76487bb2de87 16
jdawkins 0:76487bb2de87 17 int main()
jdawkins 0:76487bb2de87 18 {
jdawkins 0:76487bb2de87 19 float log_timer = 0; //initialize variable for log timer
jdawkins 0:76487bb2de87 20 bool run_flag = false;
jdawkins 0:76487bb2de87 21 bool run_ctrl = false;
jdawkins 0:76487bb2de87 22
jdawkins 0:76487bb2de87 23 mbedWSEsbcInit(9600); //Initialize the mbed WSE Project SBC
jdawkins 0:76487bb2de87 24 wait(.2); //delay at beginning for voltage settle purposes
jdawkins 0:76487bb2de87 25 t.start(); // Set up timer
jdawkins 0:76487bb2de87 26
jdawkins 1:d1a8fb968a8e 27 pc.printf("Quanser DC Motor Control Data Aquisition Program\r\n");
jdawkins 0:76487bb2de87 28
jdawkins 0:76487bb2de87 29 t.reset(); // zero timer
jdawkins 0:76487bb2de87 30 float sampT = t.read();
jdawkins 1:d1a8fb968a8e 31
jdawkins 0:76487bb2de87 32
jdawkins 0:76487bb2de87 33 while(1) {
jdawkins 0:76487bb2de87 34
jdawkins 0:76487bb2de87 35 if(pc.readable()) {
jdawkins 0:76487bb2de87 36 char c = pc.getc();
jdawkins 0:76487bb2de87 37 if(c == 'H' || c == 'h') {
jdawkins 0:76487bb2de87 38 pc.printf("Command List...\r\n");
jdawkins 0:76487bb2de87 39 pc.printf("c - run closed loop controller\r\n");
jdawkins 0:76487bb2de87 40 pc.printf("o - run open loop control\r\n");
jdawkins 0:76487bb2de87 41 pc.printf("h - display this prompt and exit\r\n");
jdawkins 0:76487bb2de87 42 pc.printf("Enter Command\r\n");
jdawkins 0:76487bb2de87 43
jdawkins 0:76487bb2de87 44 }
jdawkins 0:76487bb2de87 45 if(c == 'C' || c == 'c') {
jdawkins 0:76487bb2de87 46
jdawkins 0:76487bb2de87 47 pc.printf("Enter test duration in seconds...\r\n");
jdawkins 0:76487bb2de87 48 pc.scanf("%f",&tstop);
jdawkins 0:76487bb2de87 49 pc.printf("Test duration set to %.1f seconds.\r\n",tstop);
jdawkins 0:76487bb2de87 50
jdawkins 0:76487bb2de87 51 pc.printf("Running Closed Loop Control for %.1f seconds, press any key to stop test...\r\n",tstop);
jdawkins 0:76487bb2de87 52 run_flag = true;
jdawkins 0:76487bb2de87 53 run_ctrl = true;
jdawkins 0:76487bb2de87 54 }
jdawkins 0:76487bb2de87 55
jdawkins 0:76487bb2de87 56 if(c == 'O' || c == 'o') {
jdawkins 0:76487bb2de87 57
jdawkins 0:76487bb2de87 58 pc.printf("Enter desired duty cycle for test as a decimal between 0.00-1.00 \r\n",tstop);
jdawkins 0:76487bb2de87 59 pc.scanf("%f",&dc);
jdawkins 0:76487bb2de87 60 pc.printf("Duty Cycle is set to %.2f \r\n",dc);
jdawkins 0:76487bb2de87 61
jdawkins 0:76487bb2de87 62 pc.printf("Enter test duration in seconds...\r\n");
jdawkins 0:76487bb2de87 63 pc.scanf("%f",&tstop);
jdawkins 0:76487bb2de87 64 pc.printf("Test duration set to %.1f seconds.\r\n",tstop);
jdawkins 0:76487bb2de87 65
jdawkins 0:76487bb2de87 66 pc.printf("Running test for %.1f seconds, press any key to stop test...\r\n",tstop);
jdawkins 0:76487bb2de87 67 run_flag = true;
jdawkins 0:76487bb2de87 68 run_ctrl = false;
jdawkins 0:76487bb2de87 69 }
jdawkins 0:76487bb2de87 70
jdawkins 0:76487bb2de87 71 if(run_flag) {
jdawkins 0:76487bb2de87 72
jdawkins 0:76487bb2de87 73 wait(1);
jdawkins 0:76487bb2de87 74 t.reset();
jdawkins 1:d1a8fb968a8e 75 //Pendulum Posiiton when code starts will be the reference position
jdawkins 1:d1a8fb968a8e 76 LS7366_write_DTR(1,0); //zero encoder channel 1
jdawkins 1:d1a8fb968a8e 77 LS7366_reset_counter(1);
jdawkins 1:d1a8fb968a8e 78 LS7366_write_DTR(2,0); //zero encoder channel 1
jdawkins 1:d1a8fb968a8e 79 LS7366_reset_counter(2);
jdawkins 0:76487bb2de87 80 log_timer = 0;
jdawkins 0:76487bb2de87 81 while(t.read() < tstop) {
jdawkins 0:76487bb2de87 82
jdawkins 0:76487bb2de87 83 //Emergency Stop check
jdawkins 0:76487bb2de87 84 if(pc.readable()) { //if any key is hit turn of motor and break while loop
jdawkins 0:76487bb2de87 85 mot_control(1, 0.0);
jdawkins 0:76487bb2de87 86 break;
jdawkins 0:76487bb2de87 87 }
jdawkins 0:76487bb2de87 88
jdawkins 1:d1a8fb968a8e 89 // Read Encoder
jdawkins 1:d1a8fb968a8e 90 // Read encoder
jdawkins 1:d1a8fb968a8e 91 float enc = (float)LS7366_read_counter(2);
jdawkins 1:d1a8fb968a8e 92
jdawkins 1:d1a8fb968a8e 93 // Convert from counts to radians
jdawkins 1:d1a8fb968a8e 94 ang = 2*PI*enc/1024;
jdawkins 1:d1a8fb968a8e 95
jdawkins 1:d1a8fb968a8e 96 // Estimate States
jdawkins 0:76487bb2de87 97
jdawkins 0:76487bb2de87 98
jdawkins 0:76487bb2de87 99 if(run_ctrl) { //If controller is active
jdawkins 1:d1a8fb968a8e 100
jdawkins 0:76487bb2de87 101 // Control Law Goes Here
jdawkins 1:d1a8fb968a8e 102
jdawkins 0:76487bb2de87 103 //Convert flow rate to duty cycle;
jdawkins 0:76487bb2de87 104
jdawkins 1:d1a8fb968a8e 105 }else{ // if open loop test
jdawkins 1:d1a8fb968a8e 106
jdawkins 0:76487bb2de87 107
jdawkins 0:76487bb2de87 108 }
jdawkins 0:76487bb2de87 109
jdawkins 0:76487bb2de87 110
jdawkins 0:76487bb2de87 111 // Saturate dc command to send no more than maximum value
jdawkins 0:76487bb2de87 112 if(dc>1.0) {
jdawkins 0:76487bb2de87 113 dc =1.0;
jdawkins 0:76487bb2de87 114 }
jdawkins 0:76487bb2de87 115 if(dc<-1.0) {
jdawkins 0:76487bb2de87 116 dc = -1.0;
jdawkins 0:76487bb2de87 117 }
jdawkins 0:76487bb2de87 118
jdawkins 0:76487bb2de87 119 //Set the new output.
jdawkins 0:76487bb2de87 120
jdawkins 1:d1a8fb968a8e 121 mot_control(1, -dc); // negative value required due to polarity of motor
jdawkins 0:76487bb2de87 122
jdawkins 0:76487bb2de87 123
jdawkins 0:76487bb2de87 124 if( (t.read()- log_timer) >= (1.0/LOG_FREQ)) { //If log sample time has passed since last write
jdawkins 1:d1a8fb968a8e 125 pc.printf("%.3f, %.3f, %.3f\r\n", t.read(),ang,dc); //write data to scren
jdawkins 0:76487bb2de87 126 log_timer = t.read();
jdawkins 0:76487bb2de87 127 }
jdawkins 0:76487bb2de87 128
jdawkins 1:d1a8fb968a8e 129
jdawkins 0:76487bb2de87 130 led3=!led3; //Flash Led to Indicate program is running
jdawkins 0:76487bb2de87 131 wait(dt); //Wait for sample time of loop frequency
jdawkins 0:76487bb2de87 132 } // while t < tstop
jdawkins 0:76487bb2de87 133 mot_control(1, 0.0); // Turn off Pump once test is finished
jdawkins 0:76487bb2de87 134 run_flag = false;
jdawkins 0:76487bb2de87 135 run_ctrl = false;
jdawkins 0:76487bb2de87 136 pc.printf("\r\nEnter Command\r\n");
jdawkins 0:76487bb2de87 137 } //if run flag
jdawkins 0:76487bb2de87 138 } // if pc.readable
jdawkins 0:76487bb2de87 139 }//while
jdawkins 0:76487bb2de87 140 }//main