Program for STCL Control of motors via PWM
Dependencies: SoftPWM USBDevice mbed
Diff: main.cpp
- Revision:
- 1:2c0e9ef138a4
- Parent:
- 0:651738eb850c
- Child:
- 2:31a9ee6d066d
--- a/main.cpp Sat Feb 07 04:56:46 2015 +0000 +++ b/main.cpp Fri Apr 17 03:14:39 2015 +0000 @@ -4,16 +4,18 @@ AnalogIn Y_Axis(P0_13); AnalogIn X_Axis(P0_11); +AnalogIn Z_Axis(P0_15); SoftPWM X_PWM(P0_9); //P0_9 SoftPWM Y_PWM(P0_10); //P0_10 +SoftPWM Z_PWM(P0_4); //P0_4 USBSerial serial; #define MAX_LOGIC 10000 // Full-deflection Max Range #define MIN_LOGIC 0 // Full-deflection Min Range -#define SWITCH_LEVELS 9 // # of possible levels +#define SWITCH_LEVELS 5 // # of possible levels #define FWD 1 #define REV 0 @@ -21,17 +23,20 @@ //NOTE: Period MUST be defined 1st, them DC. -float Period_Array [SWITCH_LEVELS] = {0.010, 0.250, 1.0, 2.0, 100.0, 2.0, 1.0, 0.250, 0.010}; // PEriod Corresponding to 10, 100, 200, 400, 0 , 400, 200, 100, 10 Hz +float Period_Array [SWITCH_LEVELS] = {0.01, 0.1, 100.0, 0.1, 0.01}; // Period Corresponding to 10, 100, 0, 100, 10 Hz uint16_t SpeedThreshold = MAX_LOGIC / SWITCH_LEVELS; uint16_t X_Speed = 4; uint16_t Y_Speed = 4; +uint16_t Z_Speed = 4; -bool X_Direction; -bool Y_Direction; +bool X_Direction = FWD; +bool Y_Direction = FWD; +bool Z_Direction = FWD; uint16_t tempX_Speed = 1; uint16_t tempY_Speed = 1; +uint16_t tempZ_Speed = 1; main() @@ -44,14 +49,19 @@ Y_Speed = (uint16_t) (Y_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold / SWITCH_LEVELS); - if (X_Speed > 4) {X_Direction = FWD;} - if (X_Speed < 4) {X_Direction = REV;} - if (Y_Speed > 4) {Y_Direction = FWD;} - if (Y_Speed < 4) {Y_Direction = REV;} + Z_Speed = (uint16_t) (Z_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold / SWITCH_LEVELS); + + if (X_Speed > 2) {X_Direction = FWD;} + if (X_Speed < 2) {X_Direction = REV;} + if (Y_Speed > 2) {Y_Direction = FWD;} + if (Y_Speed < 2) {Y_Direction = REV;} + if (Z_Speed > 2) {Z_Direction = FWD;} + if (Z_Speed < 2) {Z_Direction = REV;} - //serial.printf(" X_Direction : %u", X_Direction); - //serial.printf(" Y_Direction : %u \n",Y_Direction); + serial.printf(" X_Direction : %u", X_Direction); + serial.printf(" Y_Direction : %u \n",Y_Direction); + serial.printf(" Z_Direction : %u \n",Z_Direction); if(tempX_Speed != X_Speed) { @@ -59,7 +69,7 @@ X_PWM.period(Period_Array[X_Speed]); X_PWM.write(0.5); - //serial.printf(" X_Period : %4.4f \n",Period_Array[X_Speed]); + serial.printf(" X_Period : %4.4f \n",Period_Array[X_Speed]); } if(tempY_Speed != Y_Speed) @@ -67,7 +77,15 @@ tempY_Speed = Y_Speed; Y_PWM.period(Period_Array[Y_Speed]); Y_PWM.write(0.5); - //serial.printf(" Y_Period : %4.4f \n",Period_Array[Y_Speed]); + serial.printf(" Y_Period : %4.4f \n",Period_Array[Y_Speed]); + } + + if(tempZ_Speed != Z_Speed) + { + tempZ_Speed = Z_Speed; + Z_PWM.period(Period_Array[Z_Speed]); + Z_PWM.write(0.5); + serial.printf(" Z_Period : %4.4f \n",Period_Array[Z_Speed]); } } }