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

Committer:
NKarandey
Date:
Fri Mar 24 18:40:58 2017 +0000
Revision:
57:b1b4e1379613
Parent:
53:c703e0bf5e87
Parent:
56:389436192e63
Child:
60:f171b7006c73
Resolve photointerruptor conflict

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