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@59:fcaef23bbaaf, 2017-03-24 (annotated)

Committer:
davidanderle
Date:
Fri Mar 24 18:54:18 2017 +0000
Revision:
59:fcaef23bbaaf
Parent:
53:c703e0bf5e87
Child:
60:f171b7006c73
Position control to 1 rotation tuned

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