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@0:74a5723d604a, 2017-03-13 (annotated)

Committer:
dkp14
Date:
Mon Mar 13 15:42:04 2017 +0000
Revision:
0:74a5723d604a
Child:
2:fe637a5f3387
Initial commit

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