NMLAB / Mbed 2 deprecated XYZ_Joystick_PWM

Program for STCL Control of motors via PWM

Dependencies:   SoftPWM USBDevice mbed

main.cpp@4:58ae811bff73, 2015-04-21 (annotated)

Committer:
Dzak
Date:
Tue Apr 21 02:14:24 2015 +0000
Revision:
4:58ae811bff73
Parent:
3:7a2b052c95a3
Child:
5:65c7c053e4ac
Working changes.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Dzak 0:651738eb850c 1 #include "mbed.h"
Dzak 2:31a9ee6d066d 2 //#include "USBSerial.h"
Dzak 0:651738eb850c 3 #include "SoftPWM.h"
Dzak 0:651738eb850c 4
Dzak 0:651738eb850c 5 AnalogIn Y_Axis(P0_13);
Dzak 0:651738eb850c 6 AnalogIn X_Axis(P0_11);
Dzak 1:2c0e9ef138a4 7 AnalogIn Z_Axis(P0_15);
Dzak 0:651738eb850c 8
Dzak 0:651738eb850c 9 SoftPWM X_PWM(P0_9); //P0_9
Dzak 0:651738eb850c 10 SoftPWM Y_PWM(P0_10); //P0_10
Dzak 1:2c0e9ef138a4 11 SoftPWM Z_PWM(P0_4); //P0_4
Dzak 0:651738eb850c 12
Dzak 2:31a9ee6d066d 13 //USBSerial serial;
Dzak 2:31a9ee6d066d 14
Dzak 2:31a9ee6d066d 15 Serial Send_Stage_Controller(P0_19,NC);
Dzak 2:31a9ee6d066d 16 //Serial Receive_Stage_Controller(NC,P0_18); //NULL-MOdem
Dzak 0:651738eb850c 17
Dzak 0:651738eb850c 18 #define MAX_LOGIC 10000 // Full-deflection Max Range
Dzak 0:651738eb850c 19 #define MIN_LOGIC 0 // Full-deflection Min Range
Dzak 0:651738eb850c 20
Dzak 3:7a2b052c95a3 21 #define SWITCH_LEVELS 5 // # of possible levels . MUST BE ODD. Median value is OFF
Dzak 0:651738eb850c 22 #define FWD 1
Dzak 0:651738eb850c 23 #define REV 0
Dzak 0:651738eb850c 24
Dzak 0:651738eb850c 25
Dzak 0:651738eb850c 26
Dzak 0:651738eb850c 27 //NOTE: Period MUST be defined 1st, them DC.
Dzak 0:651738eb850c 28
Dzak 4:58ae811bff73 29 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.
Dzak 3:7a2b052c95a3 30 uint16_t SpeedThreshold = MAX_LOGIC / SWITCH_LEVELS;
Dzak 0:651738eb850c 31
Dzak 4:58ae811bff73 32 uint16_t X_Speed = 2;
Dzak 4:58ae811bff73 33 uint16_t Y_Speed = 2;
Dzak 4:58ae811bff73 34 uint16_t Z_Speed = 2;
Dzak 0:651738eb850c 35
Dzak 2:31a9ee6d066d 36 char X_Direction = FWD;
Dzak 2:31a9ee6d066d 37 char Y_Direction = FWD;
Dzak 2:31a9ee6d066d 38 char Z_Direction = FWD;
Dzak 0:651738eb850c 39
Dzak 4:58ae811bff73 40 char Stop_Run = 1;
Dzak 4:58ae811bff73 41
Dzak 0:651738eb850c 42 uint16_t tempX_Speed = 1;
Dzak 0:651738eb850c 43 uint16_t tempY_Speed = 1;
Dzak 1:2c0e9ef138a4 44 uint16_t tempZ_Speed = 1;
Dzak 0:651738eb850c 45
Dzak 2:31a9ee6d066d 46 char Direction_Byte = 0x00; //This byte contains all the info about directions. USed for transfer to Stage Controller
Dzak 2:31a9ee6d066d 47
Dzak 2:31a9ee6d066d 48
Dzak 0:651738eb850c 49 main()
Dzak 0:651738eb850c 50 {
Dzak 0:651738eb850c 51 while(1)
Dzak 0:651738eb850c 52 {
Dzak 0:651738eb850c 53 wait_ms(5);
Dzak 0:651738eb850c 54
Dzak 0:651738eb850c 55 X_Speed = (uint16_t) (X_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold / SWITCH_LEVELS);
Dzak 0:651738eb850c 56
Dzak 0:651738eb850c 57 Y_Speed = (uint16_t) (Y_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold / SWITCH_LEVELS);
Dzak 0:651738eb850c 58
Dzak 1:2c0e9ef138a4 59 Z_Speed = (uint16_t) (Z_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold / SWITCH_LEVELS);
Dzak 1:2c0e9ef138a4 60
Dzak 4:58ae811bff73 61 if (X_Speed > (SWITCH_LEVELS / 2)) {X_Direction = FWD; Stop_Run = 1;}
Dzak 4:58ae811bff73 62 if (X_Speed < (SWITCH_LEVELS / 2)) {X_Direction = REV; Stop_Run = 1;}
Dzak 4:58ae811bff73 63 if (Y_Speed > (SWITCH_LEVELS / 2)) {Y_Direction = FWD; Stop_Run = 1;}
Dzak 4:58ae811bff73 64 if (Y_Speed < (SWITCH_LEVELS / 2)) {Y_Direction = REV; Stop_Run = 1;}
Dzak 4:58ae811bff73 65 if (Z_Speed > (SWITCH_LEVELS / 2)) {Z_Direction = FWD; Stop_Run = 1;}
Dzak 4:58ae811bff73 66 if (Z_Speed < (SWITCH_LEVELS / 2)) {Z_Direction = REV; Stop_Run = 1;}
Dzak 0:651738eb850c 67
Dzak 4:58ae811bff73 68 if(X_Speed == SWITCH_LEVELS / 2 && Y_Speed == SWITCH_LEVELS / 2 && Z_Speed == SWITCH_LEVELS / 2)
Dzak 4:58ae811bff73 69 {
Dzak 4:58ae811bff73 70 Stop_Run = 0; //Stopped
Dzak 4:58ae811bff73 71 }
Dzak 2:31a9ee6d066d 72
Dzak 2:31a9ee6d066d 73 /*
Dzak 2:31a9ee6d066d 74 Assemble the Direction byte for ransmission over Serial to Stage Controller. X_Dir bit 2 Y_Dir bit 1, and Z_Dir bit 0
Dzak 2:31a9ee6d066d 75 */
Dzak 2:31a9ee6d066d 76
Dzak 2:31a9ee6d066d 77
Dzak 4:58ae811bff73 78 Direction_Byte = Direction_Byte >> 4; //Clear 4 previous LSBits
Dzak 4:58ae811bff73 79 Direction_Byte = (Direction_Byte | Stop_Run) << 1; //Place Stop_Run value and shift right: Stop_Run is now bit 1
Dzak 4:58ae811bff73 80 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
Dzak 4:58ae811bff73 81 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
Dzak 4:58ae811bff73 82 Direction_Byte = (Direction_Byte | Z_Direction); //Place X_Dir value : Z_Dir is now bit 0
Dzak 2:31a9ee6d066d 83
Dzak 2:31a9ee6d066d 84 Send_Stage_Controller.putc(Direction_Byte);
Dzak 2:31a9ee6d066d 85
Dzak 2:31a9ee6d066d 86
Dzak 2:31a9ee6d066d 87 /*
Dzak 2:31a9ee6d066d 88 //============================================================================
Dzak 2:31a9ee6d066d 89 //At the receiver side the Direction_Byte will will be decoded as follows:
Dzak 2:31a9ee6d066d 90
Dzak 4:58ae811bff73 91
Dzak 2:31a9ee6d066d 92 char Mask_X = 0x04;
Dzak 2:31a9ee6d066d 93 char Mask_Y = 0x02;
Dzak 2:31a9ee6d066d 94 char Mask_Z = 0x01;
Dzak 2:31a9ee6d066d 95
Dzak 2:31a9ee6d066d 96 char Manual_Direction_Received = Receive_Stage_Controller.getc();
Dzak 2:31a9ee6d066d 97
Dzak 2:31a9ee6d066d 98 char X_Dir_Received = (Manual_Direction_Received & Mask_X) >> 2;
Dzak 2:31a9ee6d066d 99 char Y_Dir_Received = (Manual_Direction_Received & Mask_Y) >> 1;
Dzak 2:31a9ee6d066d 100 char Z_Dir_Received = Manual_Direction_Received & Mask_Z;
Dzak 2:31a9ee6d066d 101 */
Dzak 2:31a9ee6d066d 102
Dzak 2:31a9ee6d066d 103
Dzak 2:31a9ee6d066d 104 /*
Dzak 2:31a9ee6d066d 105 //=======================Only For Testing via NULL-Modem========================
Dzak 2:31a9ee6d066d 106
Dzak 2:31a9ee6d066d 107 wait(1);
Dzak 2:31a9ee6d066d 108 serial.printf("X_Direction : %u\n",X_Dir_Received);
Dzak 2:31a9ee6d066d 109 serial.printf("Y_Direction : %u\n",Y_Dir_Received);
Dzak 2:31a9ee6d066d 110 serial.printf("Z_Direction : %u\n",Z_Dir_Received);
Dzak 2:31a9ee6d066d 111
Dzak 2:31a9ee6d066d 112 */
Dzak 2:31a9ee6d066d 113 //=============================================================================
Dzak 2:31a9ee6d066d 114
Dzak 2:31a9ee6d066d 115
Dzak 2:31a9ee6d066d 116 //serial.printf(" X_Direction : %u", X_Direction);
Dzak 2:31a9ee6d066d 117 //serial.printf(" Y_Direction : %u \n",Y_Direction);
Dzak 2:31a9ee6d066d 118 //serial.printf(" Z_Direction : %u \n",Z_Direction);
Dzak 0:651738eb850c 119
Dzak 0:651738eb850c 120 if(tempX_Speed != X_Speed)
Dzak 0:651738eb850c 121 {
Dzak 0:651738eb850c 122 tempX_Speed = X_Speed;
Dzak 0:651738eb850c 123 X_PWM.period(Period_Array[X_Speed]);
Dzak 0:651738eb850c 124 X_PWM.write(0.5);
Dzak 0:651738eb850c 125
Dzak 2:31a9ee6d066d 126 //serial.printf(" X_Period : %4.4f \n",Period_Array[X_Speed]);
Dzak 0:651738eb850c 127 }
Dzak 0:651738eb850c 128
Dzak 0:651738eb850c 129 if(tempY_Speed != Y_Speed)
Dzak 0:651738eb850c 130 {
Dzak 0:651738eb850c 131 tempY_Speed = Y_Speed;
Dzak 0:651738eb850c 132 Y_PWM.period(Period_Array[Y_Speed]);
Dzak 0:651738eb850c 133 Y_PWM.write(0.5);
Dzak 2:31a9ee6d066d 134 //serial.printf(" Y_Period : %4.4f \n",Period_Array[Y_Speed]);
Dzak 1:2c0e9ef138a4 135 }
Dzak 1:2c0e9ef138a4 136
Dzak 1:2c0e9ef138a4 137 if(tempZ_Speed != Z_Speed)
Dzak 1:2c0e9ef138a4 138 {
Dzak 1:2c0e9ef138a4 139 tempZ_Speed = Z_Speed;
Dzak 1:2c0e9ef138a4 140 Z_PWM.period(Period_Array[Z_Speed]);
Dzak 1:2c0e9ef138a4 141 Z_PWM.write(0.5);
Dzak 2:31a9ee6d066d 142 //serial.printf(" Z_Period : %4.4f \n",Period_Array[Z_Speed]);
Dzak 0:651738eb850c 143 }
Dzak 0:651738eb850c 144 }
Dzak 0:651738eb850c 145 }
Dzak 0:651738eb850c 146
Dzak 0:651738eb850c 147