File content as of revision 5:65c7c053e4ac:

#include "mbed.h"
//#include "USBSerial.h"
#include "SoftPWM.h"

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;

Serial Send_Stage_Controller(P0_19,NC);    
//Serial Receive_Stage_Controller(NC,P0_18);  //NULL-MOdem

#define MAX_LOGIC  10000   // Full-deflection Max Range
#define MIN_LOGIC  0       // Full-deflection Min Range

#define SWITCH_LEVELS  5  // # of possible levels  . MUST BE ODD. Median value is OFF
#define FWD            1
#define REV            0



//NOTE: Period MUST be defined 1st, them DC. 

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 = 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;

char Direction_Byte = 0x00;   //This byte contains all the info about directions. USed for transfer to Stage Controller
    
    
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);
        
        Z_Speed = (uint16_t) (Z_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold /  SWITCH_LEVELS);
        
       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 >> 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);
        //serial.printf("%d\n",Direction_Byte);
        
        /*
        //============================================================================
        //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;
        
        char Manual_Direction_Received = Receive_Stage_Controller.getc();
        
        char X_Dir_Received = (Manual_Direction_Received & Mask_X) >> 2;
        char Y_Dir_Received = (Manual_Direction_Received & Mask_Y) >> 1;
        char Z_Dir_Received = Manual_Direction_Received & Mask_Z;
        */
        
        
        /*
        //=======================Only For Testing via NULL-Modem========================
        
        wait(1);
        serial.printf("X_Direction : %u\n",X_Dir_Received);
        serial.printf("Y_Direction : %u\n",Y_Dir_Received);
        serial.printf("Z_Direction : %u\n",Z_Dir_Received);
        
        */
        //=============================================================================
       
       
        //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)
        {
            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]);
        }
        
        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]);
        }
}
}