David Pasztor / Mbed 2 deprecated Motor_control

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed

main.cpp@31:bfb4629ca327, 2017-03-16 (annotated)

Committer:
davidanderle
Date:
Thu Mar 16 21:15:26 2017 +0000
Revision:
31:bfb4629ca327
Parent:
25:0ee6b164f234
Child:
34:599634375ba0
main put back

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dkp14 0:74a5723d604a 1 #include "mbed.h"
dkp14 0:74a5723d604a 2 #include "rtos.h"
dkp14 0:74a5723d604a 3 #include "definitions.h"
dkp14 0:74a5723d604a 4 #include "motorControl.h"
lb3314 15:b6025338e0eb 5 #include "parser.h"
dkp14 20:c3bdb8f73c02 6 #include <cmath>
dkp14 0:74a5723d604a 7
dkp14 0:74a5723d604a 8 volatile uint8_t state = 0;
davidanderle 16:d4948633c559 9 volatile float w3 = 0; //Angular velocity
dkp14 21:dd4bbb617415 10 volatile float duty = 0.5;
dkp14 4:dc705df93090 11 volatile int count_i3 = 0;
dkp14 21:dd4bbb617415 12 volatile int prev_count_i3 = -1;
davidanderle 16:d4948633c559 13 const float angularVelocities[29] = {0, 0, 0, 56.3705020, 153.953598,
davidanderle 16:d4948633c559 14 221.162308, 436.561981, 652.034058, 669.472534, 671.117249, 703.662231,
davidanderle 16:d4948633c559 15 704.767273, 695.868835, 676.609924, 689.303345, 685.318481, 680.420166,
davidanderle 16:d4948633c559 16 681.527283, 683.529175, 700.758423, 742.759216, 737.354797, 733.224365,
davidanderle 16:d4948633c559 17 716.746521, 714.952209, 717.483154, 723.597839, 727.788757, 727.957336};
dkp14 0:74a5723d604a 18
davidanderle 16:d4948633c559 19 const float dutyCycles [29] = { 0, 0.05, 0.10, 0.15, 0.20, 0.25, 0.30, 0.35,
davidanderle 16:d4948633c559 20 0.36, 0.37, 0.38, 0.39, 0.40, 0.41, 0.42, 0.43,
davidanderle 16:d4948633c559 21 0.44, 0.45, 0.50, 0.55, 0.60, 0.65, 0.70, 0.75,
davidanderle 16:d4948633c559 22 0.80, 0.85, 0.90, 0.95, 1};
davidanderle 16:d4948633c559 23 const float angle = 6.283; //2*pi for 1 revolution
dkp14 0:74a5723d604a 24 Timer dt_I3;
dkp14 0:74a5723d604a 25 Timer motorTimer;
dkp14 4:dc705df93090 26 Ticker controlTicker;
dkp14 4:dc705df93090 27
davidanderle 16:d4948633c559 28 volatile float fi0 = 0; //number of revs done
dkp14 25:0ee6b164f234 29 volatile int goalRevs = 50;
dkp14 4:dc705df93090 30 volatile float fi = 2*3.1415*goalRevs;
davidanderle 16:d4948633c559 31 volatile float goalW = 0; //desired angular velocity
dkp14 10:25d8696cb2c6 32 volatile float accError = 0;
dkp14 10:25d8696cb2c6 33 volatile float prevError = 0;
dkp14 4:dc705df93090 34
dkp14 11:043a63c952a0 35 float getDuty(float w){
davidanderle 16:d4948633c559 36 for (int i = 0; i < 28; i++) { //iterate through the angular velocities
dkp14 24:487c898c8d71 37 if (w >= angularVelocities[i] && w <= angularVelocities[i+1]) {
dkp14 11:043a63c952a0 38 if (w-angularVelocities[i] < angularVelocities[i+1]-w ) {
dkp14 11:043a63c952a0 39 return dutyCycles[i];
dkp14 11:043a63c952a0 40 }
dkp14 11:043a63c952a0 41 else {
dkp14 11:043a63c952a0 42 return dutyCycles[i+1];
dkp14 11:043a63c952a0 43 }
dkp14 11:043a63c952a0 44 }
dkp14 11:043a63c952a0 45 }
dkp14 21:dd4bbb617415 46 return dutyCycles[28];
dkp14 11:043a63c952a0 47 }
dkp14 11:043a63c952a0 48
davidanderle 31:bfb4629ca327 49 #define kp 0.0016f
davidanderle 31:bfb4629ca327 50 #define ki 0.0f //0.05f
davidanderle 31:bfb4629ca327 51 #define kd 0.00006f //0.5f
davidanderle 31:bfb4629ca327 52 #define dt 0.002f //given in ms, used to call the PID c.
davidanderle 31:bfb4629ca327 53
dkp14 4:dc705df93090 54 void control(){
davidanderle 16:d4948633c559 55 fi0 = 6.283 * count_i3; //fi0 = 2*pi*revs
dkp14 10:25d8696cb2c6 56 float error = fi - fi0;
dkp14 10:25d8696cb2c6 57 accError += error*dt;
dkp14 20:c3bdb8f73c02 58 float dError = (error - prevError)/dt;
davidanderle 31:bfb4629ca327 59 duty = kp*error + ki*accError + kd*dError;
dkp14 10:25d8696cb2c6 60 prevError = error;
davidanderle 31:bfb4629ca327 61 //if (abs(goalW) > w3) duty = 1 - w3/abs(goalW);
davidanderle 31:bfb4629ca327 62 //else duty = abs(goalW) / w3;
dkp14 25:0ee6b164f234 63 //duty = getDuty(abs(goalW));
davidanderle 31:bfb4629ca327 64 if (duty > 0) lead = -2;
dkp14 20:c3bdb8f73c02 65 else lead = 2;
dkp14 25:0ee6b164f234 66 //if (duty > 0) lead = -2;
dkp14 25:0ee6b164f234 67 //else lead = 2;
dkp14 0:74a5723d604a 68 }
dkp14 0:74a5723d604a 69
dkp14 0:74a5723d604a 70 void i3rise(){
dkp14 0:74a5723d604a 71 state = updateState();
davidanderle 2:fe637a5f3387 72 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 73
davidanderle 16:d4948633c559 74 w3 = angle/dt_I3.read(); //Calc angular velocity
dkp14 0:74a5723d604a 75
dkp14 0:74a5723d604a 76 dt_I3.reset();
dkp14 25:0ee6b164f234 77 count_i3++;
davidanderle 31:bfb4629ca327 78 //PRINT("d")
dkp14 0:74a5723d604a 79 }
dkp14 0:74a5723d604a 80
dkp14 11:043a63c952a0 81 void i_edge(){
dkp14 0:74a5723d604a 82 state = updateState();
davidanderle 2:fe637a5f3387 83 motorOut((state-orState+lead+6)%6, duty);
davidanderle 31:bfb4629ca327 84 //PRINT("a")
dkp14 0:74a5723d604a 85 }
dkp14 0:74a5723d604a 86
dkp14 0:74a5723d604a 87 void CHA_rise(){
dkp14 0:74a5723d604a 88 }
dkp14 0:74a5723d604a 89 void CHA_fall(){
dkp14 0:74a5723d604a 90 }
dkp14 0:74a5723d604a 91 void CHB_rise(){
dkp14 0:74a5723d604a 92 }
dkp14 0:74a5723d604a 93 void CHB_fall(){
dkp14 0:74a5723d604a 94 }
dkp14 0:74a5723d604a 95
dkp14 0:74a5723d604a 96 int main() {
davidanderle 16:d4948633c559 97 motorHome(); //Initialise motor before any interrupt
dkp14 0:74a5723d604a 98
davidanderle 16:d4948633c559 99 dt_I3.start(); //Start the time counters for velocity
dkp14 21:dd4bbb617415 100
dkp14 13:deb1e793f125 101 controlTicker.attach(&control, dt);
dkp14 0:74a5723d604a 102
davidanderle 16:d4948633c559 103 I1.rise(&i_edge); //Assign interrupt handlers for LEDs
dkp14 11:043a63c952a0 104 I1.fall(&i_edge);
dkp14 11:043a63c952a0 105 I2.rise(&i_edge);
dkp14 11:043a63c952a0 106 I2.fall(&i_edge);
dkp14 0:74a5723d604a 107 I3.rise(&i3rise);
dkp14 11:043a63c952a0 108 I3.fall(&i_edge);
dkp14 21:dd4bbb617415 109
dkp14 0:74a5723d604a 110 // CHA.rise(&CHA_rise);
dkp14 0:74a5723d604a 111 // CHA.fall(&CHA_fall);
dkp14 0:74a5723d604a 112 // CHB.rise(&CHB_rise);
dkp14 0:74a5723d604a 113 // CHB.fall(&CHB_fall);
dkp14 11:043a63c952a0 114
dkp14 10:25d8696cb2c6 115 state = updateState();
dkp14 11:043a63c952a0 116 motorTimer.start();
dkp14 25:0ee6b164f234 117 motorOut((state-orState+lead+6)%6, 0.3f); //Kickstart the motor
dkp14 21:dd4bbb617415 118 /*
dkp14 21:dd4bbb617415 119 while (motorTimer.read() < 30) {
dkp14 21:dd4bbb617415 120 motorOut((state-orState+1+6)%6, 0.75f);
dkp14 21:dd4bbb617415 121 wait(0.002);
dkp14 21:dd4bbb617415 122 motorOut((state-orState-1+6)%6, 0.75f);
dkp14 21:dd4bbb617415 123 wait(0.002);
dkp14 21:dd4bbb617415 124 }
dkp14 21:dd4bbb617415 125 stopMotor();
dkp14 21:dd4bbb617415 126 */
davidanderle 16:d4948633c559 127 while (count_i3 <= goalRevs) {
lb3314 18:12598db37e38 128
dkp14 20:c3bdb8f73c02 129 pc.printf("Speed: %f, goal speed: %f, duty cycle: %f, revs done: %d \n\r",w3, goalW, duty, count_i3);
dkp14 11:043a63c952a0 130 /*
dkp14 11:043a63c952a0 131 if(duty < 0.00f) {
dkp14 4:dc705df93090 132 stopMotor();
dkp14 10:25d8696cb2c6 133 return 0;
dkp14 4:dc705df93090 134 }
davidanderle 16:d4948633c559 135
dkp14 10:25d8696cb2c6 136 if(motorTimer.read() >= 30) {
dkp14 0:74a5723d604a 137 stopMotor();
dkp14 0:74a5723d604a 138 return 0;
dkp14 0:74a5723d604a 139 }
dkp14 4:dc705df93090 140 */
dkp14 0:74a5723d604a 141 }
dkp14 25:0ee6b164f234 142 while (abs(w3) > 10){
dkp14 20:c3bdb8f73c02 143 pc.printf("Speed: %f, goal speed: %f, duty cycle: %f, revs done: %d \n\r",w3, goalW, duty, count_i3);
dkp14 20:c3bdb8f73c02 144 }
dkp14 21:dd4bbb617415 145 stopMotor();
dkp14 13:deb1e793f125 146 return 0;
dkp14 0:74a5723d604a 147 }