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

Committer:
dkp14
Date:
Tue Mar 14 14:08:18 2017 +0000
Revision:
7:6bf4a61cf7c7
Parent:
4:dc705df93090
Child:
10:25d8696cb2c6
duty cycle changed to float

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