Program for STCL Control of motors via PWM
Dependencies: SoftPWM USBDevice mbed
Diff: main.cpp
- Revision:
- 4:58ae811bff73
- Parent:
- 3:7a2b052c95a3
- Child:
- 5:65c7c053e4ac
--- a/main.cpp Fri Apr 17 04:53:56 2015 +0000 +++ b/main.cpp Tue Apr 21 02:14:24 2015 +0000 @@ -26,17 +26,19 @@ //NOTE: Period MUST be defined 1st, them DC. -float Period_Array [SWITCH_LEVELS] = {0.01, 0.1, 100.0, 0.1, 0.01}; // Period Corresponding to 10, 100, 0, 100, 10 Hz MUST be odd. +float Period_Array [SWITCH_LEVELS] = {0.001, 0.01, 100.0, 0.01, 0.001}; // Period Corresponding to 10, 100, 0, 100, 10 Hz MUST be odd. uint16_t SpeedThreshold = MAX_LOGIC / SWITCH_LEVELS; -uint16_t X_Speed = 4; -uint16_t Y_Speed = 4; -uint16_t Z_Speed = 4; +uint16_t X_Speed = 2; +uint16_t Y_Speed = 2; +uint16_t Z_Speed = 2; char X_Direction = FWD; char Y_Direction = FWD; char Z_Direction = FWD; +char Stop_Run = 1; + uint16_t tempX_Speed = 1; uint16_t tempY_Speed = 1; uint16_t tempZ_Speed = 1; @@ -56,24 +58,28 @@ Z_Speed = (uint16_t) (Z_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold / SWITCH_LEVELS); - if (X_Speed > (SWITCH_LEVELS / 2)) {X_Direction = FWD;} - if (X_Speed < (SWITCH_LEVELS / 2)) {X_Direction = REV;} - if (Y_Speed > (SWITCH_LEVELS / 2)) {Y_Direction = FWD;} - if (Y_Speed < (SWITCH_LEVELS / 2)) {Y_Direction = REV;} - if (Z_Speed > (SWITCH_LEVELS / 2)) {Z_Direction = FWD;} - if (Z_Speed < (SWITCH_LEVELS / 2)) {Z_Direction = REV;} + if (X_Speed > (SWITCH_LEVELS / 2)) {X_Direction = FWD; Stop_Run = 1;} + if (X_Speed < (SWITCH_LEVELS / 2)) {X_Direction = REV; Stop_Run = 1;} + if (Y_Speed > (SWITCH_LEVELS / 2)) {Y_Direction = FWD; Stop_Run = 1;} + if (Y_Speed < (SWITCH_LEVELS / 2)) {Y_Direction = REV; Stop_Run = 1;} + if (Z_Speed > (SWITCH_LEVELS / 2)) {Z_Direction = FWD; Stop_Run = 1;} + if (Z_Speed < (SWITCH_LEVELS / 2)) {Z_Direction = REV; Stop_Run = 1;} - + if(X_Speed == SWITCH_LEVELS / 2 && Y_Speed == SWITCH_LEVELS / 2 && Z_Speed == SWITCH_LEVELS / 2) + { + Stop_Run = 0; //Stopped + } /* Assemble the Direction byte for ransmission over Serial to Stage Controller. X_Dir bit 2 Y_Dir bit 1, and Z_Dir bit 0 */ - Direction_Byte = Direction_Byte >> 3; //Clear 3 previous LSBits - Direction_Byte = (Direction_Byte | X_Direction) << 1; //Place X_Dir value and shift right: X_Dir is now bit 1 - Direction_Byte = (Direction_Byte | Y_Direction) << 1; //Place X_Dir value and shift right: Y_Dir is now bit 1, X_Dir is now bit 2 - Direction_Byte = (Direction_Byte | Z_Direction); //Place X_Dir value : Z_Dir is now bit 0, Y_Dir is now bit 1, X_Dir is now bit 2 + Direction_Byte = Direction_Byte >> 4; //Clear 4 previous LSBits + Direction_Byte = (Direction_Byte | Stop_Run) << 1; //Place Stop_Run value and shift right: Stop_Run is now bit 1 + Direction_Byte = (Direction_Byte | X_Direction) << 1; //Place X_Dir value and shift right: Stop_Run is now bit 2, X_Dir is now bit 1 + Direction_Byte = (Direction_Byte | Y_Direction) << 1; //Place X_Dir value and shift right: Stop_Run is now bit 3, X_Dir is now bit 2,Y_Dir is now bit 1 + Direction_Byte = (Direction_Byte | Z_Direction); //Place X_Dir value : Z_Dir is now bit 0 Send_Stage_Controller.putc(Direction_Byte); @@ -82,6 +88,7 @@ //============================================================================ //At the receiver side the Direction_Byte will will be decoded as follows: + char Mask_X = 0x04; char Mask_Y = 0x02; char Mask_Z = 0x01;