Program for STCL Control of motors via PWM
Dependencies: SoftPWM USBDevice mbed
Diff: main.cpp
- Revision:
- 0:651738eb850c
- Child:
- 1:2c0e9ef138a4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Sat Feb 07 04:56:46 2015 +0000 @@ -0,0 +1,75 @@ +#include "mbed.h" +#include "USBSerial.h" +#include "SoftPWM.h" + +AnalogIn Y_Axis(P0_13); +AnalogIn X_Axis(P0_11); + +SoftPWM X_PWM(P0_9); //P0_9 +SoftPWM Y_PWM(P0_10); //P0_10 + +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 FWD 1 +#define REV 0 + + + +//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 +uint16_t SpeedThreshold = MAX_LOGIC / SWITCH_LEVELS; + +uint16_t X_Speed = 4; +uint16_t Y_Speed = 4; + +bool X_Direction; +bool Y_Direction; + +uint16_t tempX_Speed = 1; +uint16_t tempY_Speed = 1; + + +main() +{ + while(1) + { + wait_ms(5); + + X_Speed = (uint16_t) (X_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold / SWITCH_LEVELS); + + 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;} + + + //serial.printf(" X_Direction : %u", X_Direction); + //serial.printf(" Y_Direction : %u \n",Y_Direction); + + if(tempX_Speed != X_Speed) + { + tempX_Speed = X_Speed; + X_PWM.period(Period_Array[X_Speed]); + X_PWM.write(0.5); + + //serial.printf(" X_Period : %4.4f \n",Period_Array[X_Speed]); + } + + if(tempY_Speed != Y_Speed) + { + 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]); + } +} +} + + \ No newline at end of file