Luke Ledva
asdfghj
Diff: MotorID.cpp
- Revision:
- 0:eb94c702a103
diff -r 000000000000 -r eb94c702a103 MotorID.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorID.cpp Tue Nov 28 04:22:29 2017 +0000
@@ -0,0 +1,231 @@
+/*************************************************************************************
+Program Name: ES305 Laboratory Experiment 1, part 1 - mbed serial setup and streaming
+Description: A basic code to run a DC motor using a Ticker object. The user specifies
+sun rimte, and duty cycle. The program runs at 100 Hz and streams data at 50 Hz
+Author: Levi DeVries, PhD, USNA
+Date: 8/14/2017
+**************************************************************************************/
+
+// Include necessary libraries
+#include "mbed.h"
+#include "mbedWSEsbc.h"
+#define PI (3.14159)
+
+
+// Declare objects (if necessary)
+Ticker Controller; // declare Ticker object named "Controller"
+DigitalOut myled(LED1); // LED, flash lights for debugging
+
+
+// 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;
+float spdErr;
+float Kp;
+float dc_p = 0.0;
+float spdErr_p = 0.0;
+
+
+
+
+// Function definition Prototypes (declarations)
+void ctrCode(); // declare that a separate (other than main) function named "ctrCode" exists
+void twoStepCode();
+void pCtrlCode();
+void PIctrlCode();
+
+
+// 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) {
+
+ // 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(&twoStepCode,Ts);
+ Controller.attach(&PIctrlCode,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()
+
+
+
+
+// Additional function definitions
+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
+ 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
+}
+
+void PIctrlCode() // 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
+ if(Time<0.1) {
+ dc = 0.0;
+ } else {
+ dc = dc_p + 0.03554*spdErr - 0.030412*spdErr_p;
+ //dc = dc_p+(0.6593/20)*(1.079*spdErr-0.9213*spdErr_p);
+ }
+ dc_p = dc;
+ spdErr_p = 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
+}
+
+