test 03/05/2019
Dependencies: mbed QEI StepperTB TEST001
Get_Control.h@1:d7aa1591fbc2, 2019-03-05 (annotated)
- Committer:
- csggreen
- Date:
- Tue Mar 05 12:57:16 2019 +0000
- Revision:
- 1:d7aa1591fbc2
- Parent:
- 0:d97ede1e24bc
green
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
csggreen | 0:d97ede1e24bc | 1 | #include "mbed.h" |
csggreen | 0:d97ede1e24bc | 2 | #include "AX12.h" |
csggreen | 0:d97ede1e24bc | 3 | #include "StepperTB.h" |
csggreen | 0:d97ede1e24bc | 4 | #include "QEI.h" |
csggreen | 0:d97ede1e24bc | 5 | |
csggreen | 0:d97ede1e24bc | 6 | Serial pc(PA_9, PB_6); |
csggreen | 0:d97ede1e24bc | 7 | AX12 myax12 (PA_11, PA_12, 1); |
csggreen | 0:d97ede1e24bc | 8 | |
csggreen | 0:d97ede1e24bc | 9 | //motor set 1 |
csggreen | 0:d97ede1e24bc | 10 | DigitalOut ENA_1(PC_8); |
csggreen | 0:d97ede1e24bc | 11 | PwmOut DIR_1(PC_6); |
csggreen | 0:d97ede1e24bc | 12 | DigitalOut PUL_1(PC_5); |
csggreen | 0:d97ede1e24bc | 13 | |
csggreen | 0:d97ede1e24bc | 14 | //motor set 2 |
csggreen | 0:d97ede1e24bc | 15 | DigitalOut ENA_2(PB_2); |
csggreen | 0:d97ede1e24bc | 16 | PwmOut DIR_2(PB_1); |
csggreen | 0:d97ede1e24bc | 17 | DigitalOut PUL_2(PB_15); |
csggreen | 0:d97ede1e24bc | 18 | |
csggreen | 0:d97ede1e24bc | 19 | //motor set 3 |
csggreen | 0:d97ede1e24bc | 20 | DigitalOut ENA_3(PC_14); |
csggreen | 0:d97ede1e24bc | 21 | PwmOut DIR_3(PC_13); |
csggreen | 0:d97ede1e24bc | 22 | DigitalOut PUL_3(PC_4); |
csggreen | 0:d97ede1e24bc | 23 | |
csggreen | 0:d97ede1e24bc | 24 | //Buzzer |
csggreen | 0:d97ede1e24bc | 25 | DigitalOut Buzzer(PB_12); |
csggreen | 0:d97ede1e24bc | 26 | |
csggreen | 0:d97ede1e24bc | 27 | //Vacum |
csggreen | 0:d97ede1e24bc | 28 | DigitalOut VACUM(PA_3); |
csggreen | 0:d97ede1e24bc | 29 | |
csggreen | 0:d97ede1e24bc | 30 | //Limit Switch |
csggreen | 0:d97ede1e24bc | 31 | DigitalOut LSwitch_1(PA_2); |
csggreen | 0:d97ede1e24bc | 32 | DigitalOut LSwitch_2(PA_10); |
csggreen | 0:d97ede1e24bc | 33 | DigitalOut LSwitch_3(PB_3); |
csggreen | 0:d97ede1e24bc | 34 | DigitalOut LSwitch_4(PB_4); |
csggreen | 0:d97ede1e24bc | 35 | DigitalOut LSwitch_5(PB_5); |
csggreen | 0:d97ede1e24bc | 36 | DigitalOut LSwitch_6(PB_10); |
csggreen | 0:d97ede1e24bc | 37 | |
csggreen | 0:d97ede1e24bc | 38 | |
csggreen | 0:d97ede1e24bc | 39 | //Encoder interrupt |
csggreen | 1:d7aa1591fbc2 | 40 | //QEI EncoderCH_1 (PC_0, PC_1, NC, 624); |
csggreen | 1:d7aa1591fbc2 | 41 | //QEI EncoderCH_2 (PB_0, PA_4, NC, 624); |
csggreen | 1:d7aa1591fbc2 | 42 | //QEI EncoderCH_3 (PA_1, PA_0, NC, 624); |
csggreen | 0:d97ede1e24bc | 43 | |
csggreen | 0:d97ede1e24bc | 44 | //////////////////////////////////////////////////////// |
csggreen | 0:d97ede1e24bc | 45 | //////////// parameter_drive ///////////// |
csggreen | 0:d97ede1e24bc | 46 | //////////////////////////////////////////////////////// |
csggreen | 0:d97ede1e24bc | 47 | long time_corexy = 0; |
csggreen | 0:d97ede1e24bc | 48 | long long time_z=0; |
csggreen | 0:d97ede1e24bc | 49 | long counter1=0; /// counter of encoder in each motor |
csggreen | 0:d97ede1e24bc | 50 | long counter2=0; |
csggreen | 0:d97ede1e24bc | 51 | long counter3=0; |
csggreen | 0:d97ede1e24bc | 52 | long CS1 =0; |
csggreen | 0:d97ede1e24bc | 53 | long CS2 =0; |
csggreen | 0:d97ede1e24bc | 54 | |
csggreen | 0:d97ede1e24bc | 55 | char direct_motor1=0; /// direct_motor=0 ---> count ++ ,direct_motor=1 ---> count -- |
csggreen | 0:d97ede1e24bc | 56 | char direct_motor2=0; |
csggreen | 0:d97ede1e24bc | 57 | char direct_motor3=0; |
csggreen | 0:d97ede1e24bc | 58 | |
csggreen | 0:d97ede1e24bc | 59 | int drive_axis = 0 ; /// drive_axis = 0 --> drive motor1 joint 1 |
csggreen | 0:d97ede1e24bc | 60 | /// drive_axis = 1 --> drive motor2 joint 2 |
csggreen | 0:d97ede1e24bc | 61 | /// drive_axis = 2 --> drive motor3 joint 3 |
csggreen | 0:d97ede1e24bc | 62 | |
csggreen | 0:d97ede1e24bc | 63 | |
csggreen | 0:d97ede1e24bc | 64 | float u_j1,u_j2,u_j3 ; |
csggreen | 0:d97ede1e24bc | 65 | ///////////////// ///////////////// |
csggreen | 0:d97ede1e24bc | 66 | ///////////////// set_ZERO ///////////////// |
csggreen | 0:d97ede1e24bc | 67 | ///////////////// ///////////////// |
csggreen | 0:d97ede1e24bc | 68 | void setzero_posi(){ |
csggreen | 0:d97ede1e24bc | 69 | } |
csggreen | 0:d97ede1e24bc | 70 | |
csggreen | 0:d97ede1e24bc | 71 | ///////////////// ///////////////// |
csggreen | 0:d97ede1e24bc | 72 | ///////////////// drive_motor ///////////////// |
csggreen | 0:d97ede1e24bc | 73 | ///////////////// ///////////////// |
csggreen | 0:d97ede1e24bc | 74 | void drive_motor_1(long u_j1){ |
csggreen | 0:d97ede1e24bc | 75 | } |
csggreen | 0:d97ede1e24bc | 76 | void drive_motor_2(long u_j2){ |
csggreen | 0:d97ede1e24bc | 77 | } |
csggreen | 0:d97ede1e24bc | 78 | void drive_motor_3(long u_j3){ |
csggreen | 0:d97ede1e24bc | 79 | } |
csggreen | 0:d97ede1e24bc | 80 |