Revision 1:da8fb81386ab, committed 2018-12-05

Comitter:

Vincent9

Date:

Wed Dec 05 05:41:06 2018 +0000

Parent:

0:980f29af9da6

Commit message:

Lab67

Changed in this revision

--- a/main.cpp	Wed Nov 14 05:55:46 2018 +0000
+++ b/main.cpp	Wed Dec 05 05:41:06 2018 +0000
@@ -1,70 +1,174 @@
+// Include necessary libraries
 #include "mbed.h"
 #include "mbedWSEsbc.h"
 #define PI (3.14159)
 
-float trt;        // Total run time
-float cet;             // Current elapsed time
-float updateperiod = 0.0083;      // Control update period (seconds) (120 Hz equivalent)
-float dsp = 0.01;     // Data streaming period (seconds) (100 Hz equivalent)
-//float ang,angp,speed;   // variables for approximating speed from encoder measurements
-//float DCAmp;            // duty cycle applied to motor
-float dc;               // duty cycle
-//long enc1;              // encoder variable
-float parameter;
-float speed;
-float DCAmp;
-float measurement;
-float rad;
-float rad_old;
+
+// Declare objects (if necessary)
+Ticker Controller; // declare Ticker object named "Controller"
+DigitalOut myled(LED1); // LED, flash lights for debugging
+
 
-Ticker Controller;
-
-void ctrCode ();
-void StepCode();
+// variables for data handling and storage
+float TotalTime;        // Total run time
+float Time;             // Current elapsed time
+float Ts = 0.0083;        // Control update period (seconds) (100 Hz equivalent)
+float Tstrm = 0.01;     // Data streaming period (seconds) (50 Hz equivalent)
+float usrDC;            // uaser-specified duty cycle
+float ang,angp,speed;   // variables for approximating speed from encoder measurements
+float dc;               // duty cycle applied to motor
+long enc1;              // encoder variable
+float lowDC;
+float dspd;             // desired speed (received from serial port)
+float spdErr;           // speed error (rad/s)
+float Kp;               // proportional control gain
 
-int main() {
-    mbedWSEsbcInit(115200);
-    mot_en1.period(.020);
-    
+
+// Function definition Prototypes (declarations)
+void ctrCode(); // declare that a separate (other than main) function named "ctrCode" exists
+void twoStepCode();
+void pCtrlCode(); // proportional controller
+
+// Enter main function
+int main ()
+{
+    // Initializes mbed to access functionality of encoder, A/D, driver, etc. chipsets
+    // Input is baud rate for PC communication
+    mbedWSEsbcInit(115200); // also initializes timer object t
+    mot_en1.period(0.020); // sets PWM period to 0.02 seconds for best DC motor operation
+
     while(1) {
-        pc.scanf("%f,%f",&trt,&DCAmp);
-        cet = 0;
-        t.reset();
-        Controller.attach(&StepCode,updateperiod);
-        t.start();
-        while (cet <= trt){
-            pc.printf("%f,%f,%f\n",cet,speed,dc);
-            wait(dsp);
-            
-            }
-            Controller.detach();
-            mot_control(1,0.0);
-            }
-        }
-    void ctrCode() {
-        cet=t.read();
-        measurement= LS7366_read_counter(1);
-        rad = (2.0*PI)/6500*measurement;
-        speed = (rad - rad_old)/updateperiod;
-        rad_old = rad;
-        
-        dc = SineAmp*sin(2.0*PI*cet/5.0);
-        mot_control(1,dc);
-        }
-        
-        void StepCode() {
-            cet=t.read();
-            measurement = LS7366_read_counter(1);
-            rad = (2.0*PI)/6500*measurement;
-            speed = (rad - rad_old)/updateperiod;
-            rad_old = rad;
-            
-            if(cet<0.1){
-                dc = 0.0;
-                } else{
-                    dc = SineAmp;
-                    }
-                    mot_control(1,dc);
-            }
-                
-        
\ No newline at end of file
+
+        // Scan serial port for user input to begin experiment
+        // pc.scanf("%f,%f",&TotalTime,&lowDC);
+        pc.scanf("%f,%f,%f",&TotalTime,&dspd,&Kp);
+
+        // perform necessary functions to time the experiment
+        Time = 0.0; // reset time variable
+        t.reset(); // reset timer object
+
+        // Attach the ctrCode function to ticker object specified with period Ts
+        Controller.attach(&pCtrlCode,Ts);
+
+        t.start(); // start measuring elapsed time
+
+        // perform operations while the elapsed time is less than the desired total time
+        while(Time <= TotalTime) {
+
+
+
+            // read current elapsed time
+            Time = t.read();
+
+
+            // send data over serial port
+            pc.printf("%f,%f,%f\n",Time,speed,dc);
+
+
+            wait(Tstrm); // print data at approximately 50 Hz
+
+
+
+
+        } // end while(Time<=Ttime)
+
+        Controller.detach(); // detach ticker to turn off controller
+        // Turn motor off at end of experiment
+        mot_control(1,0.0);
+
+    }// end while(1)
+}// end main
+
+
+
+// Additional function definitions
+void ctrCode() // function to attach to ticker
+{
+
+    myled = !myled; // toggle LED 2 to indicate control update
+
+    // Read encoder
+    enc1 = LS7366_read_counter(1); // input is the encoder channel
+
+    // Convert from counts to radians
+    ang = 2.0*PI*enc1/6400.0;
+
+    // Estimate speed
+    speed = (ang-angp)/Ts;
+
+    // Age variables
+    angp = ang;
+
+    // compute duty cycle for motor (will be changed later!)
+    dc = usrDC; // user-specified duty cycle
+
+    // motor control
+    mot_control(1,dc); // first input is the motor channel, second is duty cycle
+
+} // end ctrCode()
+
+
+void twoStepCode() // function to attach to ticker
+{
+    myled = !myled; // toggle LED 2 to indicate control update
+
+    // Read encoder
+    enc1 = LS7366_read_counter(1); // input is the encoder channel
+
+    // Convert from counts to radians
+    ang = 2.0*PI*enc1/6400.0;
+
+    // Estimate speed
+    speed = (ang-angp)/Ts;
+
+    // Age variables
+    angp = ang;
+
+    // compute duty cycle for motor over two step inputs
+    if(Time<0.1) {
+        dc = 0.0;
+    } else if(Time<0.55) {
+        dc = lowDC; // low duty cycle
+    } else {
+        dc = 0.10;
+    }
+    // motor control
+    mot_control(1,dc); // first input is the motor channel, second is duty cycle
+} // end twoStepCode()
+
+
+
+void pCtrlCode() // function to attach to ticker
+{
+    myled = !myled; // toggle LED 2 to indicate control update
+
+    // Read encoder
+    enc1 = LS7366_read_counter(1); // input is the encoder channel
+
+    // Convert from counts to radians
+    ang = 2.0*PI*enc1/6400.0;
+
+    // Estimate speed
+    speed = (ang-angp)/Ts;
+
+    // Age variables
+    angp = ang;
+
+    // compute error
+    spdErr = dspd-speed;
+
+    // compute duty cycle for motor over two step inputs
+    if(Time<0.1) {
+        dc = 0.0;
+    } else {
+        dc = Kp*(spdErr);
+    }
+    // enforce duty cycle saturation
+    if(dc>1.0) {
+        dc = 1.0;
+    } else if(dc<-1.0) {
+        dc = -1.0;
+    }
+    // motor control
+    mot_control(1,dc); // first input is the motor channel, second is duty cycle
+} // end