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@15:b6025338e0eb, 2017-03-14 (annotated)

Committer:
lb3314
Date:
Tue Mar 14 22:04:55 2017 +0000
Revision:
15:b6025338e0eb
Parent:
13:deb1e793f125
Child:
18:12598db37e38
Child:
30:e60fd3834ee7
Parser added

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 0:74a5723d604a 6
dkp14 10:25d8696cb2c6 7 #define kp 0.75f
dkp14 10:25d8696cb2c6 8 #define ki 0.5f
dkp14 10:25d8696cb2c6 9 #define kd 1.0f
dkp14 10:25d8696cb2c6 10 #define dt 0.02f //given in ms, used to call a ticker
dkp14 4:dc705df93090 11
dkp14 0:74a5723d604a 12 volatile uint8_t state = 0;
dkp14 11:043a63c952a0 13 //volatile uint8_t orState = 0; //Motor rotor offset.
dkp14 11:043a63c952a0 14 volatile float w3 = 0; //Angular velocity
dkp14 11:043a63c952a0 15 volatile float duty = 0.40;
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 11:043a63c952a0 21 const float dutyCycles [17] = {0, 0.28, 0.33, 0.38, 0.42, 0.47, 0.52, 0.57,
dkp14 11:043a63c952a0 22 0.63, 0.68, 0.73, 0.78, 0.83, 0.88, 0.93, 0.98, 1};
dkp14 0:74a5723d604a 23
dkp14 10:25d8696cb2c6 24 const float angle = 6.283; //2*pi for 1 revolution
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 11:043a63c952a0 30 volatile int goalRevs = 50;
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 11:043a63c952a0 36 float getDuty(float w){
dkp14 11:043a63c952a0 37 for (int i=0;i<16;i++) { //iterate through the angular velocities
dkp14 11:043a63c952a0 38 if (w > angularVelocities[i] && w <= angularVelocities[i+1]) {
dkp14 11:043a63c952a0 39 if (w-angularVelocities[i] < angularVelocities[i+1]-w ) {
dkp14 11:043a63c952a0 40 return dutyCycles[i];
dkp14 11:043a63c952a0 41 }
dkp14 11:043a63c952a0 42 else {
dkp14 11:043a63c952a0 43 return dutyCycles[i+1];
dkp14 11:043a63c952a0 44 }
dkp14 11:043a63c952a0 45 }
dkp14 11:043a63c952a0 46 }
dkp14 11:043a63c952a0 47 return 0;
dkp14 11:043a63c952a0 48 }
dkp14 11:043a63c952a0 49
dkp14 4:dc705df93090 50 void control(){
dkp14 4:dc705df93090 51 fi0 = 6.283 * count_i3; //fi0 = 2*pi*revs
dkp14 10:25d8696cb2c6 52 float error = fi - fi0;
dkp14 10:25d8696cb2c6 53 accError += error*dt;
dkp14 10:25d8696cb2c6 54 float dError = (prevError - error)/dt;
dkp14 4:dc705df93090 55 goalW = kp*error + ki*accError + kd*dError;
dkp14 10:25d8696cb2c6 56 prevError = error;
dkp14 11:043a63c952a0 57 duty = getDuty(goalW);
dkp14 0:74a5723d604a 58 }
dkp14 0:74a5723d604a 59
dkp14 0:74a5723d604a 60 void i3rise(){
dkp14 0:74a5723d604a 61 state = updateState();
davidanderle 2:fe637a5f3387 62 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 63
dkp14 11:043a63c952a0 64 w3 = angle/dt_I3.read(); //Calc angular velocity
dkp14 0:74a5723d604a 65
dkp14 0:74a5723d604a 66 dt_I3.reset();
dkp14 4:dc705df93090 67 count_i3++;
dkp14 0:74a5723d604a 68 }
dkp14 0:74a5723d604a 69
dkp14 11:043a63c952a0 70 void i_edge(){
dkp14 0:74a5723d604a 71 state = updateState();
davidanderle 2:fe637a5f3387 72 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 73 }
dkp14 0:74a5723d604a 74
dkp14 0:74a5723d604a 75 void CHA_rise(){
dkp14 0:74a5723d604a 76 }
dkp14 0:74a5723d604a 77 void CHA_fall(){
dkp14 0:74a5723d604a 78 }
dkp14 0:74a5723d604a 79 void CHB_rise(){
dkp14 0:74a5723d604a 80 }
dkp14 0:74a5723d604a 81 void CHB_fall(){
dkp14 0:74a5723d604a 82 }
dkp14 0:74a5723d604a 83
dkp14 0:74a5723d604a 84 int main() {
dkp14 11:043a63c952a0 85 motorHome(); //Initialise motor before any interrupt
dkp14 0:74a5723d604a 86
dkp14 11:043a63c952a0 87 dt_I3.start(); //Start the time counters for velocity
dkp14 11:043a63c952a0 88
dkp14 13:deb1e793f125 89 controlTicker.attach(&control, dt);
dkp14 0:74a5723d604a 90
dkp14 11:043a63c952a0 91 I1.rise(&i_edge); //Assign interrupt handlers for LEDs
dkp14 11:043a63c952a0 92 I1.fall(&i_edge);
dkp14 11:043a63c952a0 93 I2.rise(&i_edge);
dkp14 11:043a63c952a0 94 I2.fall(&i_edge);
dkp14 0:74a5723d604a 95 I3.rise(&i3rise);
dkp14 11:043a63c952a0 96 I3.fall(&i_edge);
dkp14 0:74a5723d604a 97 // CHA.rise(&CHA_rise);
dkp14 0:74a5723d604a 98 // CHA.fall(&CHA_fall);
dkp14 0:74a5723d604a 99 // CHB.rise(&CHB_rise);
dkp14 0:74a5723d604a 100 // CHB.fall(&CHB_fall);
dkp14 11:043a63c952a0 101
dkp14 10:25d8696cb2c6 102 state = updateState();
dkp14 11:043a63c952a0 103 motorTimer.start();
dkp14 10:25d8696cb2c6 104 motorOut((state-orState+lead+6)%6, 0.5f); //Kickstart the motor
dkp14 11:043a63c952a0 105 wait(60);
dkp14 10:25d8696cb2c6 106
dkp14 13:deb1e793f125 107 while (count_i3<=goalRevs) {
dkp14 11:043a63c952a0 108 pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
dkp14 11:043a63c952a0 109 /*
dkp14 11:043a63c952a0 110 if(duty < 0.00f) {
dkp14 4:dc705df93090 111 stopMotor();
dkp14 10:25d8696cb2c6 112 return 0;
dkp14 4:dc705df93090 113 }
dkp14 11:043a63c952a0 114 */
dkp14 7:6bf4a61cf7c7 115 /*
dkp14 10:25d8696cb2c6 116 if(motorTimer.read() >= 30) {
dkp14 0:74a5723d604a 117 stopMotor();
dkp14 0:74a5723d604a 118 return 0;
dkp14 0:74a5723d604a 119 }
dkp14 4:dc705df93090 120 */
dkp14 0:74a5723d604a 121 }
dkp14 13:deb1e793f125 122 stopMotor();
dkp14 13:deb1e793f125 123 return 0;
dkp14 0:74a5723d604a 124 }