Charlene Bissell
For Harrison
Revision 0:ae92c1cda9e8, committed 2018-04-12
- Comitter:
- m190516
- Date:
- Thu Apr 12 19:23:01 2018 +0000
- Commit message:
- lol
Changed in this revision
diff -r 000000000000 -r ae92c1cda9e8 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Apr 12 19:23:01 2018 +0000
@@ -0,0 +1,163 @@
+/*************************************************************************************
+Program Name: ES306 Laboratory Experiment - mbed serial setup and streaming
+Description: A basic code to read an analog channel and print the data to the serial port
+Author: Rich O'Brien, PhD, USNA
+Date: 13 Mar 2018
+**************************************************************************************/
+
+// Include necessary libraries
+#include "mbed.h"
+#include "mbedWSEsbc.h"
+#define PI (3.14159)
+
+// Declare necessary objects
+DigitalOut myled(LED1);
+Ticker ctrlr;
+
+// variables for data handling and storage
+float Ts = 0.01; // Sampling period 1/Ts Hz
+float TotalTime; // Total run time
+float Time; // elapsed time
+float ang; // position measured by encoder
+float ang_est; // observer states
+float speed_est; // observer states
+float ang_est_prev; // previous observer states
+float speed_est_prev; // previous observer states
+float volts; // voltage computed by control law
+float dc; // duty cycle applied to motor
+float dc_comp; // compensation for dead zone
+long enc1; // encoder variable
+float des_ang; // Desired Angle Variable
+float r; // reference For controller
+int Ncts; // number of counts = TotalTime/1.0; Ts = 1.0;
+int cts; // running counter
+
+// SF gains
+float K1; // speed gain
+float K2; // position gain
+float Kcal; // calibraiton gain
+
+
+// Observer matrix variables
+float Ad11; //top left
+float Ad12; // top right
+float Ad21; // bottom left
+float Ad22; // bottom right
+float bd1; //top
+float bd2; //bottom
+float Ld1; //top
+float Ld2; //bottom
+
+
+void open_loop_ctrl()
+{
+ // Read encoder
+ enc1 = LS7366_read_counter(1); // input is the encoder channel
+ // Convert from counts to radians
+ ang = 2*PI*enc1/6500.0;
+
+ // duty cycle
+ if (Time <0.75)
+ {dc = 0.1;}
+ else {dc = -0.1;}
+
+ //dc = des_ang;
+ // saturation
+ if (dc > 1.0) {
+ dc = 1.0;
+ }
+
+ if (dc < -1.0) {
+ dc = -1.0;
+ }
+
+ // Observer Equations for spd_est and ang_est
+
+ speed_est = Ad11*speed_est_prev+Ad12*ang_est_prev+bd1*volts+Ld1*ang;
+ ang_est = Ad21*speed_est_prev+Ad22*ang_est_prev+bd2*volts+Ld2*ang;
+
+ // Send current Duty Cycle
+ mot_control(1,dc);
+
+ // Age Variables
+ speed_est_prev = speed_est;
+ ang_est_prev = ang_est;
+}
+
+void closed_loop_ctrl()
+{
+ // Read encoder
+ enc1 = LS7366_read_counter(1); // input is the encoder channel
+ // Convert from counts to radians
+ ang = 2*PI*enc1/6500.0;
+
+ // Logic to set Desired Angle
+ if (Time <0.1){
+ r = 0;
+ }
+ else if (Time >0.1 && Time < 1.1){
+ r = des_ang;
+ }else{
+ r = 0;
+ }
+
+
+ // Observer Equations for spd_est and ang_est
+
+
+
+ // Control Law based on estimates
+
+
+ // dead zone (static friction) compensation
+
+
+ // Convert voltage to duty cycle
+
+
+ // saturation
+ if (dc > 1.0) {
+ dc = 1.0;
+ }
+
+ if (dc < -1.0) {
+ dc = -1.0;
+ }
+
+
+ mot_control(1,dc);
+ speed_est_prev = speed_est;
+ ang_est_prev = ang_est;
+}
+
+
+int main ()
+{
+ mbedWSEsbcInit(115200);
+ mot_en1.period(.020);
+ while(1) { // repeat collection cycle indefinitely
+ pc.scanf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",&TotalTime,&des_ang,&K1,&K2,&Kcal,&Ad11,&Ad12,&Ad21,&Ad22,&bd1,&bd2,&Ld1,&Ld2);
+ Ncts = floor(TotalTime/Ts);
+ cts = 0;
+ Time = 0.0;
+ speed_est_prev = 0.0;
+ ang_est_prev = 0.0;
+ speed_est = 0.0;
+ ang_est = 0.0;
+ LS7366_reset_counter(1);
+
+ ctrlr.attach(&open_loop_ctrl,Ts);// run ctrlr function every Ts sec
+ //ctrlr.attach(&closed_loop_ctrl,Ts); // run ctrlr function every Ts sec
+
+
+ while(cts <= Ncts) {
+ pc.printf("%f,%f,%f,%f,%f\n",Time,ang,ang_est,speed_est,dc);
+ cts = cts + 1;
+ Time = Time + Ts;
+ wait(Ts);
+
+ } // end while (cts <= Ncts)
+ ctrlr.detach();
+ mot_control(1,0);
+ }
+}
\ No newline at end of file
diff -r 000000000000 -r ae92c1cda9e8 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Apr 12 19:23:01 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/994bdf8177cb \ No newline at end of file
diff -r 000000000000 -r ae92c1cda9e8 mbedWSEsbc.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbedWSEsbc.lib Thu Apr 12 19:23:01 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/jebradshaw/code/mbedWSEsbc/#2564f54c89df