Vincent Angelino / Mbed 2 deprecated ES305_Lab3

a

Dependencies:   mbed mbedWSEsbc

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?

UserRevisionLine numberNew 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