Program for STCL Control of motors via PWM
Dependencies: SoftPWM USBDevice mbed
main.cpp
- Committer:
- Dzak
- Date:
- 2015-04-21
- Revision:
- 5:65c7c053e4ac
- Parent:
- 4:58ae811bff73
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]); } } }