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@2:fe637a5f3387, 2017-03-13 (annotated)

Committer:
davidanderle
Date:
Mon Mar 13 17:12:25 2017 +0000
Revision:
2:fe637a5f3387
Parent:
0:74a5723d604a
Child:
4:dc705df93090
velocity measurement

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 0:74a5723d604a 6 volatile uint8_t state = 0;
dkp14 0:74a5723d604a 7 volatile uint8_t orState = 0; //Motor rotor offset.
dkp14 0:74a5723d604a 8 volatile float w [3] = {0, 0, 0}; //Angular velocities
dkp14 0:74a5723d604a 9 volatile float avgW = 0;
davidanderle 2:fe637a5f3387 10 volatile int duty = 0;
dkp14 0:74a5723d604a 11
dkp14 0:74a5723d604a 12 const uint16_t angle = 6283; //2*pi*1000 for 1 revolution
dkp14 0:74a5723d604a 13 Timer dt_I1;
dkp14 0:74a5723d604a 14 Timer dt_I2;
dkp14 0:74a5723d604a 15 Timer dt_I3;
dkp14 0:74a5723d604a 16 Timer motorTimer;
dkp14 0:74a5723d604a 17
dkp14 0:74a5723d604a 18 void i1rise(){
dkp14 0:74a5723d604a 19 state = updateState();
davidanderle 2:fe637a5f3387 20 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 21
dkp14 0:74a5723d604a 22 dt_I1.stop();
dkp14 0:74a5723d604a 23 w[0] = angle/dt_I1.read_ms(); //Calc angular velocity
dkp14 0:74a5723d604a 24
dkp14 0:74a5723d604a 25 dt_I1.reset();
dkp14 0:74a5723d604a 26 dt_I1.start();
dkp14 0:74a5723d604a 27 }
dkp14 0:74a5723d604a 28
dkp14 0:74a5723d604a 29 void i2rise(){
dkp14 0:74a5723d604a 30 state = updateState();
davidanderle 2:fe637a5f3387 31 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 32
dkp14 0:74a5723d604a 33 dt_I2.stop();
dkp14 0:74a5723d604a 34 w[1] = angle/dt_I2.read_ms();
dkp14 0:74a5723d604a 35
dkp14 0:74a5723d604a 36 dt_I2.reset();
dkp14 0:74a5723d604a 37 dt_I2.start();
dkp14 0:74a5723d604a 38 }
dkp14 0:74a5723d604a 39
dkp14 0:74a5723d604a 40 void i3rise(){
dkp14 0:74a5723d604a 41 state = updateState();
davidanderle 2:fe637a5f3387 42 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 43
dkp14 0:74a5723d604a 44 dt_I3.stop();
dkp14 0:74a5723d604a 45 w[2] = angle/dt_I3.read_ms();
dkp14 0:74a5723d604a 46
dkp14 0:74a5723d604a 47 dt_I3.reset();
dkp14 0:74a5723d604a 48 dt_I3.start();
dkp14 0:74a5723d604a 49 }
dkp14 0:74a5723d604a 50
dkp14 0:74a5723d604a 51 void i_fall(){
dkp14 0:74a5723d604a 52 state = updateState();
davidanderle 2:fe637a5f3387 53 motorOut((state-orState+lead+6)%6, duty);
dkp14 0:74a5723d604a 54 }
dkp14 0:74a5723d604a 55
dkp14 0:74a5723d604a 56 void CHA_rise(){
dkp14 0:74a5723d604a 57 }
dkp14 0:74a5723d604a 58 void CHA_fall(){
dkp14 0:74a5723d604a 59 }
dkp14 0:74a5723d604a 60 void CHB_rise(){
dkp14 0:74a5723d604a 61 }
dkp14 0:74a5723d604a 62 void CHB_fall(){
dkp14 0:74a5723d604a 63 }
dkp14 0:74a5723d604a 64
dkp14 0:74a5723d604a 65 int main() {
dkp14 0:74a5723d604a 66 //Probably measure orState from hardware and make it a const?
dkp14 0:74a5723d604a 67 orState = motorHome(); //Initialise motor before any interrupt
dkp14 0:74a5723d604a 68
dkp14 0:74a5723d604a 69 dt_I1.start(); //Start the time counters for velocity
dkp14 0:74a5723d604a 70 dt_I2.start(); //Probably put these in an init function?
dkp14 0:74a5723d604a 71 dt_I3.start();
dkp14 0:74a5723d604a 72
davidanderle 2:fe637a5f3387 73 motorOut(4, 100); //Kickstart the motor
dkp14 0:74a5723d604a 74 motorTimer.start();
dkp14 0:74a5723d604a 75
dkp14 0:74a5723d604a 76 I1.rise(&i1rise); //Assign interrupt handlers for LEDs
dkp14 0:74a5723d604a 77 I1.fall(&i_fall);
dkp14 0:74a5723d604a 78 I2.rise(&i2rise);
dkp14 0:74a5723d604a 79 I2.fall(&i_fall);
dkp14 0:74a5723d604a 80 I3.rise(&i3rise);
dkp14 0:74a5723d604a 81 I3.fall(&i_fall);
dkp14 0:74a5723d604a 82 // CHA.rise(&CHA_rise);
dkp14 0:74a5723d604a 83 // CHA.fall(&CHA_fall);
dkp14 0:74a5723d604a 84 // CHB.rise(&CHB_rise);
dkp14 0:74a5723d604a 85 // CHB.fall(&CHB_fall);
dkp14 0:74a5723d604a 86
dkp14 0:74a5723d604a 87 while (1) {
davidanderle 2:fe637a5f3387 88 duty += 5;
davidanderle 2:fe637a5f3387 89 wait(2);
davidanderle 2:fe637a5f3387 90 if(duty == 100) {
dkp14 0:74a5723d604a 91 stopMotor();
dkp14 0:74a5723d604a 92 return 0;
dkp14 0:74a5723d604a 93 }
davidanderle 2:fe637a5f3387 94 avgW = (w[0] + w[1] + w[2])/3; //average speeds for better prediction
davidanderle 2:fe637a5f3387 95 //if(motorTimer.read_ms() >= 10000) {
davidanderle 2:fe637a5f3387 96 // stopMotor();
davidanderle 2:fe637a5f3387 97 // return 0;
davidanderle 2:fe637a5f3387 98 //}
dkp14 0:74a5723d604a 99 }
dkp14 0:74a5723d604a 100 }