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;