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

photointerruptRoutines.h@63:7921a3b553e9, 2017-03-24 (annotated)

Committer:
davidanderle
Date:
Fri Mar 24 19:57:24 2017 +0000
Revision:
63:7921a3b553e9
Parent:
60:f171b7006c73 
tune tuning

Who changed what in which revision?

UserRevisionLine numberNew contents of line
NKarandey 49:731c95cd5852 1 volatile float w3 = 0; //Angular velocity
NKarandey 49:731c95cd5852 2 volatile int count_i3 = 0;
NKarandey 49:731c95cd5852 3
NKarandey 49:731c95cd5852 4 volatile int CHA_state = 0x00;
NKarandey 49:731c95cd5852 5 volatile int CHB_state = 0x00;
NKarandey 49:731c95cd5852 6 volatile int CH_state = 0x00;
NKarandey 49:731c95cd5852 7 volatile int CH_state_prev = 0x00;
NKarandey 49:731c95cd5852 8
NKarandey 49:731c95cd5852 9 volatile float diskPosition = 0.0; //in degrees
NKarandey 49:731c95cd5852 10
NKarandey 49:731c95cd5852 11 Timer dt_I3;
davidanderle 63:7921a3b553e9 12
NKarandey 49:731c95cd5852 13
davidanderle 63:7921a3b553e9 14
NKarandey 49:731c95cd5852 15
davidanderle 59:fcaef23bbaaf 16 void updateDiskPosition() {
davidanderle 59:fcaef23bbaaf 17 if (CH_state != CH_state_prev) {
davidanderle 59:fcaef23bbaaf 18 int diff = CH_state - CH_state_prev;
davidanderle 59:fcaef23bbaaf 19
davidanderle 59:fcaef23bbaaf 20 CH_state_prev = CH_state;
davidanderle 59:fcaef23bbaaf 21 if (abs(diff) == 1 || abs(diff) == 3) {
davidanderle 59:fcaef23bbaaf 22 mutex.lock();
davidanderle 59:fcaef23bbaaf 23 if (diff < 0)
davidanderle 59:fcaef23bbaaf 24 diskPosition += angularResolution;
davidanderle 59:fcaef23bbaaf 25 else
davidanderle 59:fcaef23bbaaf 26 diskPosition -= angularResolution;
davidanderle 59:fcaef23bbaaf 27 mutex.unlock();
davidanderle 59:fcaef23bbaaf 28 }
davidanderle 59:fcaef23bbaaf 29 else if (abs(diff) == 2) {
davidanderle 59:fcaef23bbaaf 30 mutex.lock();
davidanderle 59:fcaef23bbaaf 31 if (diff < 0)
davidanderle 59:fcaef23bbaaf 32 diskPosition += 2.0f * angularResolution;
davidanderle 59:fcaef23bbaaf 33 else
davidanderle 59:fcaef23bbaaf 34 diskPosition -= 2.0f * angularResolution;
davidanderle 59:fcaef23bbaaf 35 mutex.unlock();
davidanderle 59:fcaef23bbaaf 36 }
davidanderle 59:fcaef23bbaaf 37 mutex.lock();
davidanderle 59:fcaef23bbaaf 38 if (diskPosition >= 360.0f) {
davidanderle 59:fcaef23bbaaf 39 diskPosition -= 360.0f;
davidanderle 59:fcaef23bbaaf 40 } else if (diskPosition < -360.0f) {
davidanderle 59:fcaef23bbaaf 41 diskPosition += 360.0f;
davidanderle 59:fcaef23bbaaf 42 }
davidanderle 59:fcaef23bbaaf 43 mutex.unlock();
davidanderle 59:fcaef23bbaaf 44 }
davidanderle 59:fcaef23bbaaf 45 }
davidanderle 59:fcaef23bbaaf 46
davidanderle 59:fcaef23bbaaf 47 inline void updateRelativeState() {
davidanderle 59:fcaef23bbaaf 48 CH_state = relativeStateMap[CHB_state + 2*CHA_state];
davidanderle 59:fcaef23bbaaf 49 }
NKarandey 49:731c95cd5852 50
davidanderle 59:fcaef23bbaaf 51 inline void CHA_rise() {
davidanderle 59:fcaef23bbaaf 52 CHA_state = 1;
davidanderle 59:fcaef23bbaaf 53 updateRelativeState();
davidanderle 59:fcaef23bbaaf 54 updateDiskPosition();
davidanderle 59:fcaef23bbaaf 55 }
davidanderle 59:fcaef23bbaaf 56
davidanderle 59:fcaef23bbaaf 57 // Takes 5-6us
davidanderle 59:fcaef23bbaaf 58 inline void CHA_fall() {
davidanderle 59:fcaef23bbaaf 59 CHA_state = 0;
davidanderle 59:fcaef23bbaaf 60 updateRelativeState();
davidanderle 59:fcaef23bbaaf 61 updateDiskPosition();
davidanderle 59:fcaef23bbaaf 62 }
davidanderle 59:fcaef23bbaaf 63 inline void CHB_rise() {
davidanderle 59:fcaef23bbaaf 64 CHB_state = 1;
davidanderle 59:fcaef23bbaaf 65 updateRelativeState();
davidanderle 59:fcaef23bbaaf 66 updateDiskPosition();
davidanderle 59:fcaef23bbaaf 67 }
davidanderle 59:fcaef23bbaaf 68 inline void CHB_fall() {
davidanderle 59:fcaef23bbaaf 69 CHB_state = 0;
davidanderle 59:fcaef23bbaaf 70 updateRelativeState();
davidanderle 59:fcaef23bbaaf 71 updateDiskPosition();
davidanderle 59:fcaef23bbaaf 72 }
NKarandey 49:731c95cd5852 73
NKarandey 49:731c95cd5852 74 void i_edge(){
davidanderle 63:7921a3b553e9 75 callMotorThread();
davidanderle 63:7921a3b553e9 76 //mutex.lock();
davidanderle 63:7921a3b553e9 77 // state = updateState();
davidanderle 63:7921a3b553e9 78 // mutex.unlock();
davidanderle 63:7921a3b553e9 79 // motorOut((state-orState+lead+6)%6, velocityDuty);
NKarandey 49:731c95cd5852 80 }
davidanderle 59:fcaef23bbaaf 81
davidanderle 53:c703e0bf5e87 82 void i3rise(){
davidanderle 53:c703e0bf5e87 83 mutex.lock();
davidanderle 53:c703e0bf5e87 84 w3 = 1/dt_I3.read(); //Calc angular velocity
davidanderle 53:c703e0bf5e87 85 mutex.unlock();
davidanderle 53:c703e0bf5e87 86
davidanderle 53:c703e0bf5e87 87 dt_I3.reset();
davidanderle 53:c703e0bf5e87 88
davidanderle 53:c703e0bf5e87 89 if (I2.read() == 1){ //Only count revolutions if the
davidanderle 53:c703e0bf5e87 90 mutex.lock();
davidanderle 53:c703e0bf5e87 91 count_i3++; // rotor spins forward
davidanderle 53:c703e0bf5e87 92 mutex.unlock();
davidanderle 53:c703e0bf5e87 93 }
davidanderle 63:7921a3b553e9 94 callMotorThread();
davidanderle 63:7921a3b553e9 95 // mutex.lock();
davidanderle 63:7921a3b553e9 96 // state = updateState();
davidanderle 63:7921a3b553e9 97 // mutex.unlock();
davidanderle 63:7921a3b553e9 98 // motorOut((state-orState+lead+6)%6, velocityDuty);
NKarandey 56:389436192e63 99 }
NKarandey 56:389436192e63 100
NKarandey 56:389436192e63 101 void startInterrupts() {
NKarandey 56:389436192e63 102 I1.rise(&i_edge);
NKarandey 56:389436192e63 103 I1.fall(&i_edge);
NKarandey 56:389436192e63 104 I2.rise(&i_edge);
NKarandey 56:389436192e63 105 I2.fall(&i_edge);
NKarandey 56:389436192e63 106 I3.rise(&i3rise);
NKarandey 56:389436192e63 107 I3.fall(&i_edge);
NKarandey 49:731c95cd5852 108 }