Harrison Kubena
/
ES306_Motor
a
Revision 1:ca98a3ae7a70, committed 2018-04-23
- Comitter:
- m193516
- Date:
- Mon Apr 23 01:31:28 2018 +0000
- Parent:
- 0:40605c19e1a6
- Commit message:
- Motor Experiment
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Nov 06 03:04:52 2017 +0000 +++ b/main.cpp Mon Apr 23 01:31:28 2018 +0000 @@ -1,90 +1,176 @@ -/* Harrison Kubena and Rav Dhingra -ES305 Lab 1 - Motor Ticker -27 August 2017 */ +/************************************************************************************* +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 objects -Ticker Controller; //declare ticker obj +// 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 + -//declare variables -float TotalTime; -float Time; -float Tsamp = 0.0083; -float Tstream = 0.01; -float userDC; -float ang, angp, speed; -float dc; -float enc1; -float lowDC; +// 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 ctrCode(); -void twoStepCode(); +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; +} -//enter main -int main() { - //init mbed +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; - mbedWSEsbcInit(115200); - mot_en1.period(0.020); + // 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 + + 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; + + // Control Law based on estimates + + volts = Kcal*r - K1*speed_est - K2*ang_est; + + // Convert voltage to duty cycle + + dc = volts/20; + + // dead zone (static friction) compensation - while(1) { - - pc.scanf("%f,%f", &TotalTime, &lowDC); + dc_comp = 0.06; // static friction value + /* + if (dc > 0 && abs(dc) < dc_comp) { + dc = dc_comp; + } + + if (dc < 0 && abs(dc) < dc_comp) { + dc = -1*dc_comp; + } + */ + + // 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; - t.reset(); - Controller.attach(&twoStepCode,Tsamp); - t.start(); - - //data streaming + speed_est_prev = 0.0; + ang_est_prev = 0.0; + speed_est = 0.0; + ang_est = 0.0; + LS7366_reset_counter(1); - while(Time <= TotalTime) { - - Time = t.read(); - pc.printf("%f, %f, %f\n", Time, speed, dc); - wait(Tstream); - } - Controller.detach(); - mot_control(1,0.0); - } - } -// additional function definitions -void ctrCode() { - //read encoder - enc1 = LS7366_read_counter(1); - //convert counts to rads - ang = 2.0*PI*enc1/6400.0; - //estimate speed - speed = (ang-angp)/Tsamp; - //age variables - angp = ang; - //compute duty cycle - dc = userDC; - //send the duty cycle to motor - mot_control(1,dc); } - -void twoStepCode() { - //read encoder - enc1 = LS7366_read_counter(1); - //convert counts to rads - ang = 2.0*PI*enc1/6400.0; - //estimate speed - speed = (ang-angp)/Tsamp; - //age variables - angp = ang; - //compute duty cycle - if(Time<0.1){ - dc = 0.0; - } else if(Time<0.55){ - dc = lowDC; - } else { - dc = 0.10; - } - //send the duty cycle to motor - mot_control(1,dc); } - - + // ctrlr.attach(&open_loop_ctrl,Ts);// run ctrlr function every Ts sec + ctrlr.attach(&closed_loop_ctrl,Ts); // run ctrlr function every Ts sec - \ No newline at end of file + + 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