main.cpp

00001 #include "mbed.h"
00002 //#include "USBSerial.h"
00003 #include "SoftPWM.h"
00004 
00005 AnalogIn Y_Axis(P0_13);
00006 AnalogIn X_Axis(P0_11);
00007 AnalogIn Z_Axis(P0_15);
00008 
00009 SoftPWM X_PWM(P0_9);  //P0_9
00010 SoftPWM Y_PWM(P0_10); //P0_10
00011 SoftPWM Z_PWM(P0_4); //P0_4
00012 
00013 //USBSerial serial;
00014 
00015 Serial Send_Stage_Controller(P0_19,NC);    
00016 //Serial Receive_Stage_Controller(NC,P0_18);  //NULL-MOdem
00017 
00018 #define MAX_LOGIC  10000   // Full-deflection Max Range
00019 #define MIN_LOGIC  0       // Full-deflection Min Range
00020 
00021 #define SWITCH_LEVELS  5  // # of possible levels  . MUST BE ODD. Median value is OFF
00022 #define FWD            1
00023 #define REV            0
00024 
00025 
00026 
00027 //NOTE: Period MUST be defined 1st, them DC. 
00028 
00029 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. 
00030 uint16_t SpeedThreshold = MAX_LOGIC / SWITCH_LEVELS; 
00031 
00032 uint16_t X_Speed = 2;
00033 uint16_t Y_Speed = 2;
00034 uint16_t Z_Speed = 2;
00035 
00036 char X_Direction = FWD;
00037 char Y_Direction = FWD;
00038 char Z_Direction = FWD;
00039 
00040 char Stop_Run = 1;
00041 
00042 uint16_t tempX_Speed = 1;
00043 uint16_t tempY_Speed = 1;
00044 uint16_t tempZ_Speed = 1;
00045 
00046 char Direction_Byte = 0x00;   //This byte contains all the info about directions. USed for transfer to Stage Controller
00047     
00048     
00049 main()
00050 {
00051     while(1)
00052   {
00053         wait_ms(5);
00054 
00055         X_Speed = (uint16_t) (X_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold /  SWITCH_LEVELS);
00056    
00057         Y_Speed = (uint16_t) (Y_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold /  SWITCH_LEVELS);
00058         
00059         Z_Speed = (uint16_t) (Z_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold /  SWITCH_LEVELS);
00060         
00061        if (X_Speed > (SWITCH_LEVELS / 2)) {X_Direction = FWD; Stop_Run = 1;}
00062        if (X_Speed < (SWITCH_LEVELS / 2)) {X_Direction = REV; Stop_Run = 1;}
00063        if (Y_Speed > (SWITCH_LEVELS / 2)) {Y_Direction = FWD; Stop_Run = 1;}
00064        if (Y_Speed < (SWITCH_LEVELS / 2)) {Y_Direction = REV; Stop_Run = 1;}
00065        if (Z_Speed > (SWITCH_LEVELS / 2)) {Z_Direction = FWD; Stop_Run = 1;}
00066        if (Z_Speed < (SWITCH_LEVELS / 2)) {Z_Direction = REV; Stop_Run = 1;}
00067         
00068        if((X_Speed == SWITCH_LEVELS / 2) && (Y_Speed == SWITCH_LEVELS / 2) && (Z_Speed == SWITCH_LEVELS / 2))
00069        {
00070            Stop_Run = 0;  //Stopped
00071        }
00072        
00073        /*
00074        Assemble the Direction byte for ransmission over Serial to Stage Controller. X_Dir bit 2  Y_Dir bit 1, and Z_Dir bit 0
00075        */
00076        
00077        
00078         Direction_Byte = Direction_Byte >> 4;                          //Clear 4 previous LSBits
00079         Direction_Byte = (Direction_Byte | Stop_Run) << 1;             //Place Stop_Run value and shift right: Stop_Run is now bit 1
00080         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
00081         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
00082         Direction_Byte = (Direction_Byte | Z_Direction);               //Place X_Dir value : Z_Dir is now bit 0
00083         
00084         Send_Stage_Controller.putc(Direction_Byte);
00085         //serial.printf("%d\n",Direction_Byte);
00086         
00087         /*
00088         //============================================================================
00089         //At the receiver side the Direction_Byte will will be decoded as follows:
00090         
00091         
00092         char Mask_X = 0x04;
00093         char Mask_Y = 0x02;
00094         char Mask_Z = 0x01;
00095         
00096         char Manual_Direction_Received = Receive_Stage_Controller.getc();
00097         
00098         char X_Dir_Received = (Manual_Direction_Received & Mask_X) >> 2;
00099         char Y_Dir_Received = (Manual_Direction_Received & Mask_Y) >> 1;
00100         char Z_Dir_Received = Manual_Direction_Received & Mask_Z;
00101         */
00102         
00103         
00104         /*
00105         //=======================Only For Testing via NULL-Modem========================
00106         
00107         wait(1);
00108         serial.printf("X_Direction : %u\n",X_Dir_Received);
00109         serial.printf("Y_Direction : %u\n",Y_Dir_Received);
00110         serial.printf("Z_Direction : %u\n",Z_Dir_Received);
00111         
00112         */
00113         //=============================================================================
00114        
00115        
00116         //serial.printf(" X_Direction : %u",   X_Direction);
00117         //serial.printf(" Y_Direction : %u \n",Y_Direction);
00118         //serial.printf(" Z_Direction : %u \n",Z_Direction);
00119      
00120         if(tempX_Speed != X_Speed)
00121         {
00122             tempX_Speed = X_Speed;
00123             X_PWM.period(Period_Array[X_Speed]);   
00124             X_PWM.write(0.5);  
00125             
00126         //serial.printf(" X_Period : %4.4f \n",Period_Array[X_Speed]);
00127         }
00128         
00129         if(tempY_Speed != Y_Speed)
00130         {
00131             tempY_Speed = Y_Speed;
00132             Y_PWM.period(Period_Array[Y_Speed]); 
00133             Y_PWM.write(0.5);    
00134         //serial.printf(" Y_Period : %4.4f \n",Period_Array[Y_Speed]);
00135         }
00136         
00137         if(tempZ_Speed != Z_Speed)
00138         {
00139             tempZ_Speed = Z_Speed;
00140             Z_PWM.period(Period_Array[Z_Speed]); 
00141             Z_PWM.write(0.5);    
00142         //serial.printf(" Z_Period : %4.4f \n",Period_Array[Z_Speed]);
00143         }
00144 }
00145 }
00146 
00147