USNA WRC EW306
Skeleton code for Quanser
Dependencies: mbed mbedWSEsbc
Revision 0:68715e9af246, committed 2019-04-23
- Comitter:
- jdawkins
- Date:
- Tue Apr 23 13:08:46 2019 +0000
- Commit message:
- Skeleton Code for Quanser Inverted Pendulum
Changed in this revision
diff -r 000000000000 -r 68715e9af246 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Apr 23 13:08:46 2019 +0000
@@ -0,0 +1,135 @@
+#include "mbed.h"
+#include "mbedWSEsbc.h"
+
+#define PI 3.14159
+#define LOG_FREQ 10.0
+#define CTRL_FREQ 150.0
+
+
+DigitalOut myled(LED1);
+
+float wrapToPi(float ang){
+ while(ang > PI){
+ ang-= 2*PI;
+ }
+ while(ang < -PI){
+ ang+=2*PI;
+ }
+ return ang;
+}
+
+int main()
+{
+ float log_timer = 0; //initialize variable for log timer
+ bool run_flag = false;
+ bool run_ctrl = false;
+
+ mbedWSEsbcInit(115200); //Initialize the mbed WSE Project SBC
+ wait(.2); //delay at beginning for voltage settle purposes
+ t.start(); // Set up timer
+
+ //Pendulum Posiiton when code starts will be the reference position
+ LS7366_write_DTR(1,0); //zero encoder channel 1
+ LS7366_quad_mode_x4(1);
+ LS7366_reset_counter(1);
+ LS7366_write_DTR(2,0); //zero encoder channel 1
+ LS7366_quad_mode_x4(2);
+ LS7366_reset_counter(2);
+
+ pc.printf("Quanser Inverted Pendulum Control Program\r\n");
+
+ t.reset(); // zero timer
+ float sampT = t.read();
+ float tstop = 60; //initialize stop time
+ float pwm = 0; //initialize pwm variable
+ float dt = 1/CTRL_FREQ; // set control loop sample time
+ float mot_ang;
+ float pend_ang;
+
+ while(1) {
+
+ if(pc.readable()) {
+ char c = pc.getc();
+ if(c == 'H' || c == 'h') {
+ pc.printf("Command List...\r\n");
+ pc.printf("r - run closed loop controller\r\n");
+ pc.printf("h - display this prompt and exit\r\n");
+ pc.printf("Enter Command\r\n");
+
+ }
+ if(c == 'R' || c == 'r') {
+ pc.printf("Running Pendulum lift vertical to activate controller..\r\n",tstop);
+ run_flag = true;
+ }
+
+ if(run_flag) {
+ t.reset();
+ log_timer = 0;
+ while(1) {
+
+ //Emergency Stop check
+ if(pc.readable()) { //if any key is hit turn of motor and break while loop
+ mot_control(1, 0.0);
+ break;
+ }
+
+ // read encoder 1 and encoder 2
+ float enc1 = (float)LS7366_read_counter(1);
+ float enc2 = (float)LS7366_read_counter(2);
+
+ //Convert Encoder readings to angles relative to pendulum
+ pend_ang = wrapToPi((2*PI/4096)*enc1 - PI);
+ mot_ang = -(2*PI/4096)*enc2;
+
+
+ //Logic to exit loop if Arm moves too far
+ if(abs(mot_ang) > 90*(PI/180)) {
+ pc.printf("\r\nPast Safety Limit\r\n");
+ run_ctrl = false;
+ break;
+ }
+
+ //Logic to only run control if pendulum is near vertical
+ if(abs(pend_ang) < 20*(PI/180)){
+ run_ctrl = true;
+ }else{
+ run_ctrl = false;
+ }
+
+ if(run_ctrl) { //If controller is active
+ // Control Law goes here!!
+ pwm = 0.0;
+
+ }else{
+ pwm = 0.0;
+ }
+
+
+ // Saturate PWM command to send no more than maximum value
+ if(pwm>1.0) {
+ pwm =1.0;
+ }
+ if(pwm<-1.0) {
+ pwm = -1.0;
+ }
+
+ //Set the new output.
+ mot_control(1, pwm);
+
+
+ if((t.read()- log_timer) >= (1.0/LOG_FREQ)) { //If log sample time has passed since last write
+ pc.printf("%.2f, %.3f, %.3f, %.3f\r\n", t.read(),pend_ang,mot_ang,pwm); //write data to scren
+ log_timer = t.read();
+ }
+
+ led3=!led3; //Flash Led to Indicate program is running
+ wait(dt); //Wait for sample time of loop frequency
+ } // while t < tstop
+ mot_control(1, 0.0); // Turn off Motor once test is finished
+ run_flag = false;
+ run_ctrl = false;
+ pc.printf("\r\nEnter Command\r\n");
+ } //if run flag
+ } // if pc.readable
+ }//while
+}//main
diff -r 000000000000 -r 68715e9af246 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Apr 23 13:08:46 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/487b796308b0 \ No newline at end of file
diff -r 000000000000 -r 68715e9af246 mbedWSEsbc.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbedWSEsbc.lib Tue Apr 23 13:08:46 2019 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/jebradshaw/code/mbedWSEsbc/#9041603c430e