Vincent Angelino
/
ES305_Lab3
a
main.cpp@1:da8fb81386ab, 2018-12-05 (annotated)
- Committer:
- Vincent9
- Date:
- Wed Dec 05 05:41:06 2018 +0000
- Revision:
- 1:da8fb81386ab
- Parent:
- 0:980f29af9da6
Lab67
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Vincent9 | 1:da8fb81386ab | 1 | // Include necessary libraries |
Vincent9 | 0:980f29af9da6 | 2 | #include "mbed.h" |
Vincent9 | 0:980f29af9da6 | 3 | #include "mbedWSEsbc.h" |
Vincent9 | 0:980f29af9da6 | 4 | #define PI (3.14159) |
Vincent9 | 0:980f29af9da6 | 5 | |
Vincent9 | 1:da8fb81386ab | 6 | |
Vincent9 | 1:da8fb81386ab | 7 | // Declare objects (if necessary) |
Vincent9 | 1:da8fb81386ab | 8 | Ticker Controller; // declare Ticker object named "Controller" |
Vincent9 | 1:da8fb81386ab | 9 | DigitalOut myled(LED1); // LED, flash lights for debugging |
Vincent9 | 1:da8fb81386ab | 10 | |
Vincent9 | 0:980f29af9da6 | 11 | |
Vincent9 | 1:da8fb81386ab | 12 | // variables for data handling and storage |
Vincent9 | 1:da8fb81386ab | 13 | float TotalTime; // Total run time |
Vincent9 | 1:da8fb81386ab | 14 | float Time; // Current elapsed time |
Vincent9 | 1:da8fb81386ab | 15 | float Ts = 0.0083; // Control update period (seconds) (100 Hz equivalent) |
Vincent9 | 1:da8fb81386ab | 16 | float Tstrm = 0.01; // Data streaming period (seconds) (50 Hz equivalent) |
Vincent9 | 1:da8fb81386ab | 17 | float usrDC; // uaser-specified duty cycle |
Vincent9 | 1:da8fb81386ab | 18 | float ang,angp,speed; // variables for approximating speed from encoder measurements |
Vincent9 | 1:da8fb81386ab | 19 | float dc; // duty cycle applied to motor |
Vincent9 | 1:da8fb81386ab | 20 | long enc1; // encoder variable |
Vincent9 | 1:da8fb81386ab | 21 | float lowDC; |
Vincent9 | 1:da8fb81386ab | 22 | float dspd; // desired speed (received from serial port) |
Vincent9 | 1:da8fb81386ab | 23 | float spdErr; // speed error (rad/s) |
Vincent9 | 1:da8fb81386ab | 24 | float Kp; // proportional control gain |
Vincent9 | 0:980f29af9da6 | 25 | |
Vincent9 | 1:da8fb81386ab | 26 | |
Vincent9 | 1:da8fb81386ab | 27 | // Function definition Prototypes (declarations) |
Vincent9 | 1:da8fb81386ab | 28 | void ctrCode(); // declare that a separate (other than main) function named "ctrCode" exists |
Vincent9 | 1:da8fb81386ab | 29 | void twoStepCode(); |
Vincent9 | 1:da8fb81386ab | 30 | void pCtrlCode(); // proportional controller |
Vincent9 | 1:da8fb81386ab | 31 | |
Vincent9 | 1:da8fb81386ab | 32 | // Enter main function |
Vincent9 | 1:da8fb81386ab | 33 | int main () |
Vincent9 | 1:da8fb81386ab | 34 | { |
Vincent9 | 1:da8fb81386ab | 35 | // Initializes mbed to access functionality of encoder, A/D, driver, etc. chipsets |
Vincent9 | 1:da8fb81386ab | 36 | // Input is baud rate for PC communication |
Vincent9 | 1:da8fb81386ab | 37 | mbedWSEsbcInit(115200); // also initializes timer object t |
Vincent9 | 1:da8fb81386ab | 38 | mot_en1.period(0.020); // sets PWM period to 0.02 seconds for best DC motor operation |
Vincent9 | 1:da8fb81386ab | 39 | |
Vincent9 | 0:980f29af9da6 | 40 | while(1) { |
Vincent9 | 1:da8fb81386ab | 41 | |
Vincent9 | 1:da8fb81386ab | 42 | // Scan serial port for user input to begin experiment |
Vincent9 | 1:da8fb81386ab | 43 | // pc.scanf("%f,%f",&TotalTime,&lowDC); |
Vincent9 | 1:da8fb81386ab | 44 | pc.scanf("%f,%f,%f",&TotalTime,&dspd,&Kp); |
Vincent9 | 1:da8fb81386ab | 45 | |
Vincent9 | 1:da8fb81386ab | 46 | // perform necessary functions to time the experiment |
Vincent9 | 1:da8fb81386ab | 47 | Time = 0.0; // reset time variable |
Vincent9 | 1:da8fb81386ab | 48 | t.reset(); // reset timer object |
Vincent9 | 1:da8fb81386ab | 49 | |
Vincent9 | 1:da8fb81386ab | 50 | // Attach the ctrCode function to ticker object specified with period Ts |
Vincent9 | 1:da8fb81386ab | 51 | Controller.attach(&pCtrlCode,Ts); |
Vincent9 | 1:da8fb81386ab | 52 | |
Vincent9 | 1:da8fb81386ab | 53 | t.start(); // start measuring elapsed time |
Vincent9 | 1:da8fb81386ab | 54 | |
Vincent9 | 1:da8fb81386ab | 55 | // perform operations while the elapsed time is less than the desired total time |
Vincent9 | 1:da8fb81386ab | 56 | while(Time <= TotalTime) { |
Vincent9 | 1:da8fb81386ab | 57 | |
Vincent9 | 1:da8fb81386ab | 58 | |
Vincent9 | 1:da8fb81386ab | 59 | |
Vincent9 | 1:da8fb81386ab | 60 | // read current elapsed time |
Vincent9 | 1:da8fb81386ab | 61 | Time = t.read(); |
Vincent9 | 1:da8fb81386ab | 62 | |
Vincent9 | 1:da8fb81386ab | 63 | |
Vincent9 | 1:da8fb81386ab | 64 | // send data over serial port |
Vincent9 | 1:da8fb81386ab | 65 | pc.printf("%f,%f,%f\n",Time,speed,dc); |
Vincent9 | 1:da8fb81386ab | 66 | |
Vincent9 | 1:da8fb81386ab | 67 | |
Vincent9 | 1:da8fb81386ab | 68 | wait(Tstrm); // print data at approximately 50 Hz |
Vincent9 | 1:da8fb81386ab | 69 | |
Vincent9 | 1:da8fb81386ab | 70 | |
Vincent9 | 1:da8fb81386ab | 71 | |
Vincent9 | 1:da8fb81386ab | 72 | |
Vincent9 | 1:da8fb81386ab | 73 | } // end while(Time<=Ttime) |
Vincent9 | 1:da8fb81386ab | 74 | |
Vincent9 | 1:da8fb81386ab | 75 | Controller.detach(); // detach ticker to turn off controller |
Vincent9 | 1:da8fb81386ab | 76 | // Turn motor off at end of experiment |
Vincent9 | 1:da8fb81386ab | 77 | mot_control(1,0.0); |
Vincent9 | 1:da8fb81386ab | 78 | |
Vincent9 | 1:da8fb81386ab | 79 | }// end while(1) |
Vincent9 | 1:da8fb81386ab | 80 | }// end main |
Vincent9 | 1:da8fb81386ab | 81 | |
Vincent9 | 1:da8fb81386ab | 82 | |
Vincent9 | 1:da8fb81386ab | 83 | |
Vincent9 | 1:da8fb81386ab | 84 | // Additional function definitions |
Vincent9 | 1:da8fb81386ab | 85 | void ctrCode() // function to attach to ticker |
Vincent9 | 1:da8fb81386ab | 86 | { |
Vincent9 | 1:da8fb81386ab | 87 | |
Vincent9 | 1:da8fb81386ab | 88 | myled = !myled; // toggle LED 2 to indicate control update |
Vincent9 | 1:da8fb81386ab | 89 | |
Vincent9 | 1:da8fb81386ab | 90 | // Read encoder |
Vincent9 | 1:da8fb81386ab | 91 | enc1 = LS7366_read_counter(1); // input is the encoder channel |
Vincent9 | 1:da8fb81386ab | 92 | |
Vincent9 | 1:da8fb81386ab | 93 | // Convert from counts to radians |
Vincent9 | 1:da8fb81386ab | 94 | ang = 2.0*PI*enc1/6400.0; |
Vincent9 | 1:da8fb81386ab | 95 | |
Vincent9 | 1:da8fb81386ab | 96 | // Estimate speed |
Vincent9 | 1:da8fb81386ab | 97 | speed = (ang-angp)/Ts; |
Vincent9 | 1:da8fb81386ab | 98 | |
Vincent9 | 1:da8fb81386ab | 99 | // Age variables |
Vincent9 | 1:da8fb81386ab | 100 | angp = ang; |
Vincent9 | 1:da8fb81386ab | 101 | |
Vincent9 | 1:da8fb81386ab | 102 | // compute duty cycle for motor (will be changed later!) |
Vincent9 | 1:da8fb81386ab | 103 | dc = usrDC; // user-specified duty cycle |
Vincent9 | 1:da8fb81386ab | 104 | |
Vincent9 | 1:da8fb81386ab | 105 | // motor control |
Vincent9 | 1:da8fb81386ab | 106 | mot_control(1,dc); // first input is the motor channel, second is duty cycle |
Vincent9 | 1:da8fb81386ab | 107 | |
Vincent9 | 1:da8fb81386ab | 108 | } // end ctrCode() |
Vincent9 | 1:da8fb81386ab | 109 | |
Vincent9 | 1:da8fb81386ab | 110 | |
Vincent9 | 1:da8fb81386ab | 111 | void twoStepCode() // function to attach to ticker |
Vincent9 | 1:da8fb81386ab | 112 | { |
Vincent9 | 1:da8fb81386ab | 113 | myled = !myled; // toggle LED 2 to indicate control update |
Vincent9 | 1:da8fb81386ab | 114 | |
Vincent9 | 1:da8fb81386ab | 115 | // Read encoder |
Vincent9 | 1:da8fb81386ab | 116 | enc1 = LS7366_read_counter(1); // input is the encoder channel |
Vincent9 | 1:da8fb81386ab | 117 | |
Vincent9 | 1:da8fb81386ab | 118 | // Convert from counts to radians |
Vincent9 | 1:da8fb81386ab | 119 | ang = 2.0*PI*enc1/6400.0; |
Vincent9 | 1:da8fb81386ab | 120 | |
Vincent9 | 1:da8fb81386ab | 121 | // Estimate speed |
Vincent9 | 1:da8fb81386ab | 122 | speed = (ang-angp)/Ts; |
Vincent9 | 1:da8fb81386ab | 123 | |
Vincent9 | 1:da8fb81386ab | 124 | // Age variables |
Vincent9 | 1:da8fb81386ab | 125 | angp = ang; |
Vincent9 | 1:da8fb81386ab | 126 | |
Vincent9 | 1:da8fb81386ab | 127 | // compute duty cycle for motor over two step inputs |
Vincent9 | 1:da8fb81386ab | 128 | if(Time<0.1) { |
Vincent9 | 1:da8fb81386ab | 129 | dc = 0.0; |
Vincent9 | 1:da8fb81386ab | 130 | } else if(Time<0.55) { |
Vincent9 | 1:da8fb81386ab | 131 | dc = lowDC; // low duty cycle |
Vincent9 | 1:da8fb81386ab | 132 | } else { |
Vincent9 | 1:da8fb81386ab | 133 | dc = 0.10; |
Vincent9 | 1:da8fb81386ab | 134 | } |
Vincent9 | 1:da8fb81386ab | 135 | // motor control |
Vincent9 | 1:da8fb81386ab | 136 | mot_control(1,dc); // first input is the motor channel, second is duty cycle |
Vincent9 | 1:da8fb81386ab | 137 | } // end twoStepCode() |
Vincent9 | 1:da8fb81386ab | 138 | |
Vincent9 | 1:da8fb81386ab | 139 | |
Vincent9 | 1:da8fb81386ab | 140 | |
Vincent9 | 1:da8fb81386ab | 141 | void pCtrlCode() // function to attach to ticker |
Vincent9 | 1:da8fb81386ab | 142 | { |
Vincent9 | 1:da8fb81386ab | 143 | myled = !myled; // toggle LED 2 to indicate control update |
Vincent9 | 1:da8fb81386ab | 144 | |
Vincent9 | 1:da8fb81386ab | 145 | // Read encoder |
Vincent9 | 1:da8fb81386ab | 146 | enc1 = LS7366_read_counter(1); // input is the encoder channel |
Vincent9 | 1:da8fb81386ab | 147 | |
Vincent9 | 1:da8fb81386ab | 148 | // Convert from counts to radians |
Vincent9 | 1:da8fb81386ab | 149 | ang = 2.0*PI*enc1/6400.0; |
Vincent9 | 1:da8fb81386ab | 150 | |
Vincent9 | 1:da8fb81386ab | 151 | // Estimate speed |
Vincent9 | 1:da8fb81386ab | 152 | speed = (ang-angp)/Ts; |
Vincent9 | 1:da8fb81386ab | 153 | |
Vincent9 | 1:da8fb81386ab | 154 | // Age variables |
Vincent9 | 1:da8fb81386ab | 155 | angp = ang; |
Vincent9 | 1:da8fb81386ab | 156 | |
Vincent9 | 1:da8fb81386ab | 157 | // compute error |
Vincent9 | 1:da8fb81386ab | 158 | spdErr = dspd-speed; |
Vincent9 | 1:da8fb81386ab | 159 | |
Vincent9 | 1:da8fb81386ab | 160 | // compute duty cycle for motor over two step inputs |
Vincent9 | 1:da8fb81386ab | 161 | if(Time<0.1) { |
Vincent9 | 1:da8fb81386ab | 162 | dc = 0.0; |
Vincent9 | 1:da8fb81386ab | 163 | } else { |
Vincent9 | 1:da8fb81386ab | 164 | dc = Kp*(spdErr); |
Vincent9 | 1:da8fb81386ab | 165 | } |
Vincent9 | 1:da8fb81386ab | 166 | // enforce duty cycle saturation |
Vincent9 | 1:da8fb81386ab | 167 | if(dc>1.0) { |
Vincent9 | 1:da8fb81386ab | 168 | dc = 1.0; |
Vincent9 | 1:da8fb81386ab | 169 | } else if(dc<-1.0) { |
Vincent9 | 1:da8fb81386ab | 170 | dc = -1.0; |
Vincent9 | 1:da8fb81386ab | 171 | } |
Vincent9 | 1:da8fb81386ab | 172 | // motor control |
Vincent9 | 1:da8fb81386ab | 173 | mot_control(1,dc); // first input is the motor channel, second is duty cycle |
Vincent9 | 1:da8fb81386ab | 174 | } // end |