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@10:25d8696cb2c6, 2017-03-14 (annotated)

Committer:
dkp14
Date:
Tue Mar 14 17:38:46 2017 +0000
Revision:
10:25d8696cb2c6
Parent:
7:6bf4a61cf7c7
Child:
11:043a63c952a0
controller implemented

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"
dkp14 0:74a5723d604a 5
dkp14 10:25d8696cb2c6 6 #define kp 0.75f
dkp14 10:25d8696cb2c6 7 #define ki 0.5f
dkp14 10:25d8696cb2c6 8 #define kd 1.0f
dkp14 10:25d8696cb2c6 9 #define dt 0.02f //given in ms, used to call a ticker
dkp14 4:dc705df93090 10
dkp14 0:74a5723d604a 11 volatile uint8_t state = 0;
dkp14 0:74a5723d604a 12 volatile uint8_t orState = 0; //Motor rotor offset.
dkp14 0:74a5723d604a 13 volatile float w [3] = {0, 0, 0}; //Angular velocities
dkp14 0:74a5723d604a 14 volatile float avgW = 0;
dkp14 10:25d8696cb2c6 15 volatile float duty = 0.28;
dkp14 4:dc705df93090 16 volatile int count_i3 = 0;
dkp14 10:25d8696cb2c6 17 const float angularVelocities[17] = {0, 112.355598, 164.975998, 218.721725,
dkp14 10:25d8696cb2c6 18 260.672943, 291.491364, 308.479126, 316.805908, 321.183929, 324.010712,
dkp14 10:25d8696cb2c6 19 326.146759, 336.187103, 351.175629, 364.887604, 377.856659, 387.58432,
dkp14 10:25d8696cb2c6 20 392.540314};
dkp14 0:74a5723d604a 21
dkp14 10:25d8696cb2c6 22 const float angle = 6.283; //2*pi for 1 revolution
dkp14 0:74a5723d604a 23 Timer dt_I1;
dkp14 0:74a5723d604a 24 Timer dt_I2;
dkp14 0:74a5723d604a 25 Timer dt_I3;
dkp14 0:74a5723d604a 26 Timer motorTimer;
dkp14 4:dc705df93090 27 Ticker controlTicker;
dkp14 4:dc705df93090 28
dkp14 4:dc705df93090 29 volatile float fi0 = 0; //number of revs done
dkp14 4:dc705df93090 30 volatile int goalRevs = 20;
dkp14 4:dc705df93090 31 volatile float fi = 2*3.1415*goalRevs;
dkp14 4:dc705df93090 32 volatile float goalW = 0; //desired angular velocity
dkp14 10:25d8696cb2c6 33 volatile float accError = 0;
dkp14 10:25d8696cb2c6 34 volatile float prevError = 0;
dkp14 4:dc705df93090 35
dkp14 4:dc705df93090 36 void control(){
dkp14 4:dc705df93090 37 fi0 = 6.283 * count_i3; //fi0 = 2*pi*revs
dkp14 10:25d8696cb2c6 38 float error = fi - fi0;
dkp14 10:25d8696cb2c6 39 accError += error*dt;
dkp14 10:25d8696cb2c6 40 float dError = (prevError - error)/dt;
dkp14 4:dc705df93090 41 goalW = kp*error + ki*accError + kd*dError;
dkp14 10:25d8696cb2c6 42 prevError = error;
dkp14 4:dc705df93090 43 }
dkp14 10:25d8696cb2c6 44
dkp14 0:74a5723d604a 45
dkp14 0:74a5723d604a 46 void i1rise(){
dkp14 0:74a5723d604a 47 state = updateState();
davidanderle 2:fe637a5f3387 48 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 49
dkp14 10:25d8696cb2c6 50 w[0] = angle/dt_I1.read(); //Calc angular velocity
dkp14 10:25d8696cb2c6 51 dt_I1.reset();
dkp14 0:74a5723d604a 52 }
dkp14 0:74a5723d604a 53
dkp14 0:74a5723d604a 54 void i2rise(){
dkp14 0:74a5723d604a 55 state = updateState();
davidanderle 2:fe637a5f3387 56 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 57
dkp14 10:25d8696cb2c6 58 w[1] = angle/dt_I2.read();
dkp14 10:25d8696cb2c6 59 dt_I2.reset();
dkp14 0:74a5723d604a 60 }
dkp14 0:74a5723d604a 61
dkp14 0:74a5723d604a 62 void i3rise(){
dkp14 0:74a5723d604a 63 state = updateState();
davidanderle 2:fe637a5f3387 64 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 65
dkp14 10:25d8696cb2c6 66 w[2] = angle/dt_I3.read();
dkp14 0:74a5723d604a 67
dkp14 0:74a5723d604a 68 dt_I3.reset();
dkp14 4:dc705df93090 69 count_i3++;
dkp14 0:74a5723d604a 70 }
dkp14 0:74a5723d604a 71
dkp14 0:74a5723d604a 72 void i_fall(){
dkp14 0:74a5723d604a 73 state = updateState();
davidanderle 2:fe637a5f3387 74 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 75 }
dkp14 0:74a5723d604a 76
dkp14 0:74a5723d604a 77 void CHA_rise(){
dkp14 0:74a5723d604a 78 }
dkp14 0:74a5723d604a 79 void CHA_fall(){
dkp14 0:74a5723d604a 80 }
dkp14 0:74a5723d604a 81 void CHB_rise(){
dkp14 0:74a5723d604a 82 }
dkp14 0:74a5723d604a 83 void CHB_fall(){
dkp14 0:74a5723d604a 84 }
dkp14 0:74a5723d604a 85
dkp14 0:74a5723d604a 86 int main() {
dkp14 0:74a5723d604a 87 //Probably measure orState from hardware and make it a const?
dkp14 0:74a5723d604a 88 orState = motorHome(); //Initialise motor before any interrupt
dkp14 10:25d8696cb2c6 89 pc.printf("orState: %d \n\r", orState);
dkp14 0:74a5723d604a 90
dkp14 0:74a5723d604a 91 dt_I1.start(); //Start the time counters for velocity
dkp14 0:74a5723d604a 92 dt_I2.start(); //Probably put these in an init function?
dkp14 0:74a5723d604a 93 dt_I3.start();
dkp14 0:74a5723d604a 94
dkp14 0:74a5723d604a 95 motorTimer.start();
dkp14 0:74a5723d604a 96
dkp14 10:25d8696cb2c6 97 controlTicker.attach(&control, dt);
dkp14 4:dc705df93090 98
dkp14 0:74a5723d604a 99 I1.rise(&i1rise); //Assign interrupt handlers for LEDs
dkp14 0:74a5723d604a 100 I1.fall(&i_fall);
dkp14 0:74a5723d604a 101 I2.rise(&i2rise);
dkp14 0:74a5723d604a 102 I2.fall(&i_fall);
dkp14 0:74a5723d604a 103 I3.rise(&i3rise);
dkp14 0:74a5723d604a 104 I3.fall(&i_fall);
dkp14 0:74a5723d604a 105 // CHA.rise(&CHA_rise);
dkp14 0:74a5723d604a 106 // CHA.fall(&CHA_fall);
dkp14 0:74a5723d604a 107 // CHB.rise(&CHB_rise);
dkp14 0:74a5723d604a 108 // CHB.fall(&CHB_fall);
dkp14 10:25d8696cb2c6 109 state = updateState();
dkp14 10:25d8696cb2c6 110 motorOut((state-orState+lead+6)%6, 0.5f); //Kickstart the motor
dkp14 10:25d8696cb2c6 111 wait(30);
dkp14 10:25d8696cb2c6 112
dkp14 0:74a5723d604a 113 while (1) {
dkp14 10:25d8696cb2c6 114 avgW = (w[0] + w[1] + w[2])/3; //average speeds for better prediction
dkp14 10:25d8696cb2c6 115 pc.printf("Speed: %f, duty cycle: %f \n\r",avgW, duty);
dkp14 10:25d8696cb2c6 116
dkp14 10:25d8696cb2c6 117 duty += 0.05f;
dkp14 10:25d8696cb2c6 118 wait(10);
dkp14 10:25d8696cb2c6 119 if(duty > 1.05f) {
dkp14 4:dc705df93090 120 stopMotor();
dkp14 10:25d8696cb2c6 121 return 0;
dkp14 4:dc705df93090 122 }
dkp14 7:6bf4a61cf7c7 123 /*
dkp14 10:25d8696cb2c6 124 if(motorTimer.read() >= 30) {
dkp14 0:74a5723d604a 125 stopMotor();
dkp14 0:74a5723d604a 126 return 0;
dkp14 0:74a5723d604a 127 }
dkp14 4:dc705df93090 128 */
dkp14 0:74a5723d604a 129 }
dkp14 0:74a5723d604a 130 }