USNA WRC EW306 / Mbed 2 deprecated inverted_pendulum_mids

Skeleton code for Quanser

Dependencies:   mbed mbedWSEsbc

main.cpp@0:68715e9af246, 2019-04-23 (annotated)

Committer:
jdawkins
Date:
Tue Apr 23 13:08:46 2019 +0000
Revision:
0:68715e9af246
Skeleton Code for Quanser Inverted Pendulum

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 0:68715e9af246 1 #include "mbed.h"
jdawkins 0:68715e9af246 2 #include "mbedWSEsbc.h"
jdawkins 0:68715e9af246 3
jdawkins 0:68715e9af246 4 #define PI 3.14159
jdawkins 0:68715e9af246 5 #define LOG_FREQ 10.0
jdawkins 0:68715e9af246 6 #define CTRL_FREQ 150.0
jdawkins 0:68715e9af246 7
jdawkins 0:68715e9af246 8
jdawkins 0:68715e9af246 9 DigitalOut myled(LED1);
jdawkins 0:68715e9af246 10
jdawkins 0:68715e9af246 11 float wrapToPi(float ang){
jdawkins 0:68715e9af246 12 while(ang > PI){
jdawkins 0:68715e9af246 13 ang-= 2*PI;
jdawkins 0:68715e9af246 14 }
jdawkins 0:68715e9af246 15 while(ang < -PI){
jdawkins 0:68715e9af246 16 ang+=2*PI;
jdawkins 0:68715e9af246 17 }
jdawkins 0:68715e9af246 18 return ang;
jdawkins 0:68715e9af246 19 }
jdawkins 0:68715e9af246 20
jdawkins 0:68715e9af246 21 int main()
jdawkins 0:68715e9af246 22 {
jdawkins 0:68715e9af246 23 float log_timer = 0; //initialize variable for log timer
jdawkins 0:68715e9af246 24 bool run_flag = false;
jdawkins 0:68715e9af246 25 bool run_ctrl = false;
jdawkins 0:68715e9af246 26
jdawkins 0:68715e9af246 27 mbedWSEsbcInit(115200); //Initialize the mbed WSE Project SBC
jdawkins 0:68715e9af246 28 wait(.2); //delay at beginning for voltage settle purposes
jdawkins 0:68715e9af246 29 t.start(); // Set up timer
jdawkins 0:68715e9af246 30
jdawkins 0:68715e9af246 31 //Pendulum Posiiton when code starts will be the reference position
jdawkins 0:68715e9af246 32 LS7366_write_DTR(1,0); //zero encoder channel 1
jdawkins 0:68715e9af246 33 LS7366_quad_mode_x4(1);
jdawkins 0:68715e9af246 34 LS7366_reset_counter(1);
jdawkins 0:68715e9af246 35 LS7366_write_DTR(2,0); //zero encoder channel 1
jdawkins 0:68715e9af246 36 LS7366_quad_mode_x4(2);
jdawkins 0:68715e9af246 37 LS7366_reset_counter(2);
jdawkins 0:68715e9af246 38
jdawkins 0:68715e9af246 39 pc.printf("Quanser Inverted Pendulum Control Program\r\n");
jdawkins 0:68715e9af246 40
jdawkins 0:68715e9af246 41 t.reset(); // zero timer
jdawkins 0:68715e9af246 42 float sampT = t.read();
jdawkins 0:68715e9af246 43 float tstop = 60; //initialize stop time
jdawkins 0:68715e9af246 44 float pwm = 0; //initialize pwm variable
jdawkins 0:68715e9af246 45 float dt = 1/CTRL_FREQ; // set control loop sample time
jdawkins 0:68715e9af246 46 float mot_ang;
jdawkins 0:68715e9af246 47 float pend_ang;
jdawkins 0:68715e9af246 48
jdawkins 0:68715e9af246 49 while(1) {
jdawkins 0:68715e9af246 50
jdawkins 0:68715e9af246 51 if(pc.readable()) {
jdawkins 0:68715e9af246 52 char c = pc.getc();
jdawkins 0:68715e9af246 53 if(c == 'H' || c == 'h') {
jdawkins 0:68715e9af246 54 pc.printf("Command List...\r\n");
jdawkins 0:68715e9af246 55 pc.printf("r - run closed loop controller\r\n");
jdawkins 0:68715e9af246 56 pc.printf("h - display this prompt and exit\r\n");
jdawkins 0:68715e9af246 57 pc.printf("Enter Command\r\n");
jdawkins 0:68715e9af246 58
jdawkins 0:68715e9af246 59 }
jdawkins 0:68715e9af246 60 if(c == 'R' || c == 'r') {
jdawkins 0:68715e9af246 61 pc.printf("Running Pendulum lift vertical to activate controller..\r\n",tstop);
jdawkins 0:68715e9af246 62 run_flag = true;
jdawkins 0:68715e9af246 63 }
jdawkins 0:68715e9af246 64
jdawkins 0:68715e9af246 65 if(run_flag) {
jdawkins 0:68715e9af246 66 t.reset();
jdawkins 0:68715e9af246 67 log_timer = 0;
jdawkins 0:68715e9af246 68 while(1) {
jdawkins 0:68715e9af246 69
jdawkins 0:68715e9af246 70 //Emergency Stop check
jdawkins 0:68715e9af246 71 if(pc.readable()) { //if any key is hit turn of motor and break while loop
jdawkins 0:68715e9af246 72 mot_control(1, 0.0);
jdawkins 0:68715e9af246 73 break;
jdawkins 0:68715e9af246 74 }
jdawkins 0:68715e9af246 75
jdawkins 0:68715e9af246 76 // read encoder 1 and encoder 2
jdawkins 0:68715e9af246 77 float enc1 = (float)LS7366_read_counter(1);
jdawkins 0:68715e9af246 78 float enc2 = (float)LS7366_read_counter(2);
jdawkins 0:68715e9af246 79
jdawkins 0:68715e9af246 80 //Convert Encoder readings to angles relative to pendulum
jdawkins 0:68715e9af246 81 pend_ang = wrapToPi((2*PI/4096)*enc1 - PI);
jdawkins 0:68715e9af246 82 mot_ang = -(2*PI/4096)*enc2;
jdawkins 0:68715e9af246 83
jdawkins 0:68715e9af246 84
jdawkins 0:68715e9af246 85 //Logic to exit loop if Arm moves too far
jdawkins 0:68715e9af246 86 if(abs(mot_ang) > 90*(PI/180)) {
jdawkins 0:68715e9af246 87 pc.printf("\r\nPast Safety Limit\r\n");
jdawkins 0:68715e9af246 88 run_ctrl = false;
jdawkins 0:68715e9af246 89 break;
jdawkins 0:68715e9af246 90 }
jdawkins 0:68715e9af246 91
jdawkins 0:68715e9af246 92 //Logic to only run control if pendulum is near vertical
jdawkins 0:68715e9af246 93 if(abs(pend_ang) < 20*(PI/180)){
jdawkins 0:68715e9af246 94 run_ctrl = true;
jdawkins 0:68715e9af246 95 }else{
jdawkins 0:68715e9af246 96 run_ctrl = false;
jdawkins 0:68715e9af246 97 }
jdawkins 0:68715e9af246 98
jdawkins 0:68715e9af246 99 if(run_ctrl) { //If controller is active
jdawkins 0:68715e9af246 100 // Control Law goes here!!
jdawkins 0:68715e9af246 101 pwm = 0.0;
jdawkins 0:68715e9af246 102
jdawkins 0:68715e9af246 103 }else{
jdawkins 0:68715e9af246 104 pwm = 0.0;
jdawkins 0:68715e9af246 105 }
jdawkins 0:68715e9af246 106
jdawkins 0:68715e9af246 107
jdawkins 0:68715e9af246 108 // Saturate PWM command to send no more than maximum value
jdawkins 0:68715e9af246 109 if(pwm>1.0) {
jdawkins 0:68715e9af246 110 pwm =1.0;
jdawkins 0:68715e9af246 111 }
jdawkins 0:68715e9af246 112 if(pwm<-1.0) {
jdawkins 0:68715e9af246 113 pwm = -1.0;
jdawkins 0:68715e9af246 114 }
jdawkins 0:68715e9af246 115
jdawkins 0:68715e9af246 116 //Set the new output.
jdawkins 0:68715e9af246 117 mot_control(1, pwm);
jdawkins 0:68715e9af246 118
jdawkins 0:68715e9af246 119
jdawkins 0:68715e9af246 120 if((t.read()- log_timer) >= (1.0/LOG_FREQ)) { //If log sample time has passed since last write
jdawkins 0:68715e9af246 121 pc.printf("%.2f, %.3f, %.3f, %.3f\r\n", t.read(),pend_ang,mot_ang,pwm); //write data to scren
jdawkins 0:68715e9af246 122 log_timer = t.read();
jdawkins 0:68715e9af246 123 }
jdawkins 0:68715e9af246 124
jdawkins 0:68715e9af246 125 led3=!led3; //Flash Led to Indicate program is running
jdawkins 0:68715e9af246 126 wait(dt); //Wait for sample time of loop frequency
jdawkins 0:68715e9af246 127 } // while t < tstop
jdawkins 0:68715e9af246 128 mot_control(1, 0.0); // Turn off Motor once test is finished
jdawkins 0:68715e9af246 129 run_flag = false;
jdawkins 0:68715e9af246 130 run_ctrl = false;
jdawkins 0:68715e9af246 131 pc.printf("\r\nEnter Command\r\n");
jdawkins 0:68715e9af246 132 } //if run flag
jdawkins 0:68715e9af246 133 } // if pc.readable
jdawkins 0:68715e9af246 134 }//while
jdawkins 0:68715e9af246 135 }//main