hsu han-lin
/
RoboticArmIO
For Sewing Robot use
main.cpp@0:9b0f6c2eaea2, 2018-07-13 (annotated)
- Committer:
- stanley1228
- Date:
- Fri Jul 13 06:30:03 2018 +0000
- Revision:
- 0:9b0f6c2eaea2
For sewing robot use
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
stanley1228 | 0:9b0f6c2eaea2 | 1 | #include "mbed.h" |
stanley1228 | 0:9b0f6c2eaea2 | 2 | |
stanley1228 | 0:9b0f6c2eaea2 | 3 | //------------------------------------ |
stanley1228 | 0:9b0f6c2eaea2 | 4 | // Hyperterminal configuration |
stanley1228 | 0:9b0f6c2eaea2 | 5 | // 9600 bauds, 8-bit data, no parity |
stanley1228 | 0:9b0f6c2eaea2 | 6 | //------------------------------------ |
stanley1228 | 0:9b0f6c2eaea2 | 7 | |
stanley1228 | 0:9b0f6c2eaea2 | 8 | Serial pc(SERIAL_TX, SERIAL_RX); |
stanley1228 | 0:9b0f6c2eaea2 | 9 | Serial se3(PC_10,PC_11); //left top of CN7,right top of CN7(MB1136) |
stanley1228 | 0:9b0f6c2eaea2 | 10 | //DigitalOut myled(LED1); |
stanley1228 | 0:9b0f6c2eaea2 | 11 | |
stanley1228 | 0:9b0f6c2eaea2 | 12 | //==pin define==// |
stanley1228 | 0:9b0f6c2eaea2 | 13 | //rotate motor |
stanley1228 | 0:9b0f6c2eaea2 | 14 | DigitalOut pin_en(D2); |
stanley1228 | 0:9b0f6c2eaea2 | 15 | DigitalOut pin_dir(D3); |
stanley1228 | 0:9b0f6c2eaea2 | 16 | DigitalOut pin_step(D4); |
stanley1228 | 0:9b0f6c2eaea2 | 17 | |
stanley1228 | 0:9b0f6c2eaea2 | 18 | //gripper |
stanley1228 | 0:9b0f6c2eaea2 | 19 | PwmOut pin_grp_pwmL(D10); |
stanley1228 | 0:9b0f6c2eaea2 | 20 | DigitalOut pin_grpL_1(D8); |
stanley1228 | 0:9b0f6c2eaea2 | 21 | DigitalOut pin_grpL_2(D9); |
stanley1228 | 0:9b0f6c2eaea2 | 22 | PwmOut pin_grp_pwmR(D5); |
stanley1228 | 0:9b0f6c2eaea2 | 23 | DigitalOut pin_grpR_1(D7); |
stanley1228 | 0:9b0f6c2eaea2 | 24 | DigitalOut pin_grpR_2(D6); |
stanley1228 | 0:9b0f6c2eaea2 | 25 | |
stanley1228 | 0:9b0f6c2eaea2 | 26 | |
stanley1228 | 0:9b0f6c2eaea2 | 27 | |
stanley1228 | 0:9b0f6c2eaea2 | 28 | //foot lifter |
stanley1228 | 0:9b0f6c2eaea2 | 29 | DigitalOut pin_footlifter(D12); |
stanley1228 | 0:9b0f6c2eaea2 | 30 | |
stanley1228 | 0:9b0f6c2eaea2 | 31 | //spindle |
stanley1228 | 0:9b0f6c2eaea2 | 32 | AnalogOut pin_spindle(PA_4); //A2 |
stanley1228 | 0:9b0f6c2eaea2 | 33 | |
stanley1228 | 0:9b0f6c2eaea2 | 34 | //trimmer |
stanley1228 | 0:9b0f6c2eaea2 | 35 | DigitalOut pin_trimmer(D13); |
stanley1228 | 0:9b0f6c2eaea2 | 36 | //================// |
stanley1228 | 0:9b0f6c2eaea2 | 37 | //==Rotate Motor==// |
stanley1228 | 0:9b0f6c2eaea2 | 38 | //================// |
stanley1228 | 0:9b0f6c2eaea2 | 39 | #define DEF_STEP_M_EN false |
stanley1228 | 0:9b0f6c2eaea2 | 40 | #define DEF_STEP_M_DIS true |
stanley1228 | 0:9b0f6c2eaea2 | 41 | #define DEF_CLOCK_WISE true |
stanley1228 | 0:9b0f6c2eaea2 | 42 | #define DEF_COUNTER_CLOCK_WISE false |
stanley1228 | 0:9b0f6c2eaea2 | 43 | |
stanley1228 | 0:9b0f6c2eaea2 | 44 | //腰身圓盤72格/圈 72格/360 = 1格/5deg = 0.2格/deg |
stanley1228 | 0:9b0f6c2eaea2 | 45 | //馬達9格/圈 ->1格/40deg |
stanley1228 | 0:9b0f6c2eaea2 | 46 | //EX:轉腰90deg -> 腰轉90*0.2=18格 -> 馬達轉18*40=720degree = 720/(1.8)*16=400*16=6400 step |
stanley1228 | 0:9b0f6c2eaea2 | 47 | |
stanley1228 | 0:9b0f6c2eaea2 | 48 | #define DEF_DEG_TO_STEPS (16/1.8) |
stanley1228 | 0:9b0f6c2eaea2 | 49 | #define DEF_WAIST_GRID_PER_DEG 0.2f |
stanley1228 | 0:9b0f6c2eaea2 | 50 | #define DEF_STEP_M_DEG_PER_GRID 40 |
stanley1228 | 0:9b0f6c2eaea2 | 51 | |
stanley1228 | 0:9b0f6c2eaea2 | 52 | int RotateMotor(bool dir,float deg) |
stanley1228 | 0:9b0f6c2eaea2 | 53 | { |
stanley1228 | 0:9b0f6c2eaea2 | 54 | int steps=deg*DEF_WAIST_GRID_PER_DEG*DEF_STEP_M_DEG_PER_GRID*DEF_DEG_TO_STEPS; |
stanley1228 | 0:9b0f6c2eaea2 | 55 | |
stanley1228 | 0:9b0f6c2eaea2 | 56 | //pc.printf("steps=%d\n",steps); |
stanley1228 | 0:9b0f6c2eaea2 | 57 | |
stanley1228 | 0:9b0f6c2eaea2 | 58 | pin_dir=dir; |
stanley1228 | 0:9b0f6c2eaea2 | 59 | |
stanley1228 | 0:9b0f6c2eaea2 | 60 | pin_en=DEF_STEP_M_EN; |
stanley1228 | 0:9b0f6c2eaea2 | 61 | |
stanley1228 | 0:9b0f6c2eaea2 | 62 | for(int i=0;i<steps;i++) |
stanley1228 | 0:9b0f6c2eaea2 | 63 | { |
stanley1228 | 0:9b0f6c2eaea2 | 64 | pin_step=true; |
stanley1228 | 0:9b0f6c2eaea2 | 65 | wait_ms(1); |
stanley1228 | 0:9b0f6c2eaea2 | 66 | pin_step=false; |
stanley1228 | 0:9b0f6c2eaea2 | 67 | wait_ms(1); |
stanley1228 | 0:9b0f6c2eaea2 | 68 | } |
stanley1228 | 0:9b0f6c2eaea2 | 69 | |
stanley1228 | 0:9b0f6c2eaea2 | 70 | |
stanley1228 | 0:9b0f6c2eaea2 | 71 | pin_en=DEF_STEP_M_DIS; |
stanley1228 | 0:9b0f6c2eaea2 | 72 | return 0; |
stanley1228 | 0:9b0f6c2eaea2 | 73 | } |
stanley1228 | 0:9b0f6c2eaea2 | 74 | |
stanley1228 | 0:9b0f6c2eaea2 | 75 | //============// |
stanley1228 | 0:9b0f6c2eaea2 | 76 | //==Gripper==// |
stanley1228 | 0:9b0f6c2eaea2 | 77 | //============// |
stanley1228 | 0:9b0f6c2eaea2 | 78 | #define DEF_RELEASE 0 |
stanley1228 | 0:9b0f6c2eaea2 | 79 | #define DEF_HOLD 1 |
stanley1228 | 0:9b0f6c2eaea2 | 80 | #define DEF_RIGHT_HAND 1 |
stanley1228 | 0:9b0f6c2eaea2 | 81 | #define DEF_LEFT_HAND 2 |
stanley1228 | 0:9b0f6c2eaea2 | 82 | |
stanley1228 | 0:9b0f6c2eaea2 | 83 | void initial_TB6612FNG_Gripper() |
stanley1228 | 0:9b0f6c2eaea2 | 84 | { |
stanley1228 | 0:9b0f6c2eaea2 | 85 | // Set PWM |
stanley1228 | 0:9b0f6c2eaea2 | 86 | pin_grp_pwmL.period_ms(2); |
stanley1228 | 0:9b0f6c2eaea2 | 87 | pin_grp_pwmL.write(0.5); |
stanley1228 | 0:9b0f6c2eaea2 | 88 | |
stanley1228 | 0:9b0f6c2eaea2 | 89 | pin_grp_pwmR.period_ms(2); |
stanley1228 | 0:9b0f6c2eaea2 | 90 | pin_grp_pwmR.write(0.5); |
stanley1228 | 0:9b0f6c2eaea2 | 91 | } |
stanley1228 | 0:9b0f6c2eaea2 | 92 | void TB6612FNG_Gripper(int rh,bool hold,int delay_ms) |
stanley1228 | 0:9b0f6c2eaea2 | 93 | { |
stanley1228 | 0:9b0f6c2eaea2 | 94 | if(rh==DEF_RIGHT_HAND) |
stanley1228 | 0:9b0f6c2eaea2 | 95 | { |
stanley1228 | 0:9b0f6c2eaea2 | 96 | if(hold==true) |
stanley1228 | 0:9b0f6c2eaea2 | 97 | { |
stanley1228 | 0:9b0f6c2eaea2 | 98 | pin_grpR_1=1; |
stanley1228 | 0:9b0f6c2eaea2 | 99 | pin_grpR_2=0; |
stanley1228 | 0:9b0f6c2eaea2 | 100 | wait_ms(delay_ms); |
stanley1228 | 0:9b0f6c2eaea2 | 101 | } |
stanley1228 | 0:9b0f6c2eaea2 | 102 | else |
stanley1228 | 0:9b0f6c2eaea2 | 103 | { |
stanley1228 | 0:9b0f6c2eaea2 | 104 | pin_grpR_1=0; |
stanley1228 | 0:9b0f6c2eaea2 | 105 | pin_grpR_2=1; |
stanley1228 | 0:9b0f6c2eaea2 | 106 | wait_ms(delay_ms); |
stanley1228 | 0:9b0f6c2eaea2 | 107 | } |
stanley1228 | 0:9b0f6c2eaea2 | 108 | pin_grpR_1=0; |
stanley1228 | 0:9b0f6c2eaea2 | 109 | pin_grpR_2=0; |
stanley1228 | 0:9b0f6c2eaea2 | 110 | |
stanley1228 | 0:9b0f6c2eaea2 | 111 | } |
stanley1228 | 0:9b0f6c2eaea2 | 112 | else if(rh==DEF_LEFT_HAND) |
stanley1228 | 0:9b0f6c2eaea2 | 113 | { |
stanley1228 | 0:9b0f6c2eaea2 | 114 | if(hold==true) |
stanley1228 | 0:9b0f6c2eaea2 | 115 | { |
stanley1228 | 0:9b0f6c2eaea2 | 116 | pin_grpL_1=1; |
stanley1228 | 0:9b0f6c2eaea2 | 117 | pin_grpL_2=0; |
stanley1228 | 0:9b0f6c2eaea2 | 118 | wait_ms(delay_ms); |
stanley1228 | 0:9b0f6c2eaea2 | 119 | } |
stanley1228 | 0:9b0f6c2eaea2 | 120 | else |
stanley1228 | 0:9b0f6c2eaea2 | 121 | { |
stanley1228 | 0:9b0f6c2eaea2 | 122 | pin_grpL_1=0; |
stanley1228 | 0:9b0f6c2eaea2 | 123 | pin_grpL_2=1; |
stanley1228 | 0:9b0f6c2eaea2 | 124 | wait_ms(delay_ms); |
stanley1228 | 0:9b0f6c2eaea2 | 125 | } |
stanley1228 | 0:9b0f6c2eaea2 | 126 | pin_grpL_1=0; |
stanley1228 | 0:9b0f6c2eaea2 | 127 | pin_grpL_2=0; |
stanley1228 | 0:9b0f6c2eaea2 | 128 | } |
stanley1228 | 0:9b0f6c2eaea2 | 129 | } |
stanley1228 | 0:9b0f6c2eaea2 | 130 | |
stanley1228 | 0:9b0f6c2eaea2 | 131 | |
stanley1228 | 0:9b0f6c2eaea2 | 132 | //==================== |
stanley1228 | 0:9b0f6c2eaea2 | 133 | //communication struct |
stanley1228 | 0:9b0f6c2eaea2 | 134 | //==================== |
stanley1228 | 0:9b0f6c2eaea2 | 135 | #define DEF_CMD_ROTATE_MOTOR 0xa1 |
stanley1228 | 0:9b0f6c2eaea2 | 136 | #define DEF_CMD_GRIP 0xa2 |
stanley1228 | 0:9b0f6c2eaea2 | 137 | #define DEF_CMD_FL 0xa3 |
stanley1228 | 0:9b0f6c2eaea2 | 138 | #define DEF_CMD_SPINDLE 0xa4 |
stanley1228 | 0:9b0f6c2eaea2 | 139 | #define DEF_CMD_TRIMMER 0xa5 |
stanley1228 | 0:9b0f6c2eaea2 | 140 | |
stanley1228 | 0:9b0f6c2eaea2 | 141 | #define DEF_IX_PARA_DIR 0 |
stanley1228 | 0:9b0f6c2eaea2 | 142 | #define DEF_IX_PARA_DEG_HIGH 1 |
stanley1228 | 0:9b0f6c2eaea2 | 143 | #define DEF_IX_PARA_DEG_LOW 2 |
stanley1228 | 0:9b0f6c2eaea2 | 144 | #define DEF_ECHO_ROTATE_MOTOR_DONE 3 |
stanley1228 | 0:9b0f6c2eaea2 | 145 | |
stanley1228 | 0:9b0f6c2eaea2 | 146 | #define DEF_IX_PARA_GRP_LR 0 |
stanley1228 | 0:9b0f6c2eaea2 | 147 | #define DEF_IX_PARA_GRP_HOLD 1 |
stanley1228 | 0:9b0f6c2eaea2 | 148 | #define DEF_IX_PARA_GRP_DELAY_H 2 |
stanley1228 | 0:9b0f6c2eaea2 | 149 | #define DEF_IX_PARA_GRP_DELAY_L 3 |
stanley1228 | 0:9b0f6c2eaea2 | 150 | |
stanley1228 | 0:9b0f6c2eaea2 | 151 | |
stanley1228 | 0:9b0f6c2eaea2 | 152 | |
stanley1228 | 0:9b0f6c2eaea2 | 153 | |
stanley1228 | 0:9b0f6c2eaea2 | 154 | |
stanley1228 | 0:9b0f6c2eaea2 | 155 | int main() |
stanley1228 | 0:9b0f6c2eaea2 | 156 | { |
stanley1228 | 0:9b0f6c2eaea2 | 157 | char para[4]={0}; |
stanley1228 | 0:9b0f6c2eaea2 | 158 | |
stanley1228 | 0:9b0f6c2eaea2 | 159 | initial_TB6612FNG_Gripper(); |
stanley1228 | 0:9b0f6c2eaea2 | 160 | |
stanley1228 | 0:9b0f6c2eaea2 | 161 | //pc.printf("start\n"); |
stanley1228 | 0:9b0f6c2eaea2 | 162 | |
stanley1228 | 0:9b0f6c2eaea2 | 163 | //TB6612FNG_Gripper(DEF_LEFT_HAND,false,500); |
stanley1228 | 0:9b0f6c2eaea2 | 164 | //RotateMotor(DEF_COUNTER_CLOCK_WISE,90); |
stanley1228 | 0:9b0f6c2eaea2 | 165 | |
stanley1228 | 0:9b0f6c2eaea2 | 166 | while(1) |
stanley1228 | 0:9b0f6c2eaea2 | 167 | { |
stanley1228 | 0:9b0f6c2eaea2 | 168 | if(se3.readable()) |
stanley1228 | 0:9b0f6c2eaea2 | 169 | { |
stanley1228 | 0:9b0f6c2eaea2 | 170 | char cmd; |
stanley1228 | 0:9b0f6c2eaea2 | 171 | cmd=se3.getc(); |
stanley1228 | 0:9b0f6c2eaea2 | 172 | |
stanley1228 | 0:9b0f6c2eaea2 | 173 | |
stanley1228 | 0:9b0f6c2eaea2 | 174 | if(cmd==DEF_CMD_ROTATE_MOTOR) |
stanley1228 | 0:9b0f6c2eaea2 | 175 | { |
stanley1228 | 0:9b0f6c2eaea2 | 176 | //pc.printf("DEF_CMD_ROTATE_MOTOR\n"); |
stanley1228 | 0:9b0f6c2eaea2 | 177 | |
stanley1228 | 0:9b0f6c2eaea2 | 178 | for(int i=0;i<3;i++) |
stanley1228 | 0:9b0f6c2eaea2 | 179 | { |
stanley1228 | 0:9b0f6c2eaea2 | 180 | para[i]=se3.getc(); |
stanley1228 | 0:9b0f6c2eaea2 | 181 | } |
stanley1228 | 0:9b0f6c2eaea2 | 182 | bool dir=(para[DEF_IX_PARA_DIR]==1)? DEF_CLOCK_WISE:DEF_COUNTER_CLOCK_WISE; |
stanley1228 | 0:9b0f6c2eaea2 | 183 | int deg=para[DEF_IX_PARA_DEG_HIGH]<<8 | para[DEF_IX_PARA_DEG_LOW]; |
stanley1228 | 0:9b0f6c2eaea2 | 184 | |
stanley1228 | 0:9b0f6c2eaea2 | 185 | //pc.printf("dir=%d\n",dir); |
stanley1228 | 0:9b0f6c2eaea2 | 186 | //pc.printf("deg=%d\n",deg); |
stanley1228 | 0:9b0f6c2eaea2 | 187 | |
stanley1228 | 0:9b0f6c2eaea2 | 188 | RotateMotor(dir,deg); |
stanley1228 | 0:9b0f6c2eaea2 | 189 | |
stanley1228 | 0:9b0f6c2eaea2 | 190 | //pc.printf("after RotateMotor\n"); |
stanley1228 | 0:9b0f6c2eaea2 | 191 | //echo to master after rotate done |
stanley1228 | 0:9b0f6c2eaea2 | 192 | cmd=DEF_ECHO_ROTATE_MOTOR_DONE; |
stanley1228 | 0:9b0f6c2eaea2 | 193 | se3.putc(cmd); |
stanley1228 | 0:9b0f6c2eaea2 | 194 | |
stanley1228 | 0:9b0f6c2eaea2 | 195 | } |
stanley1228 | 0:9b0f6c2eaea2 | 196 | else if(cmd==DEF_CMD_GRIP) |
stanley1228 | 0:9b0f6c2eaea2 | 197 | { |
stanley1228 | 0:9b0f6c2eaea2 | 198 | for(int i=0;i<4;i++) |
stanley1228 | 0:9b0f6c2eaea2 | 199 | { |
stanley1228 | 0:9b0f6c2eaea2 | 200 | para[i]=se3.getc(); |
stanley1228 | 0:9b0f6c2eaea2 | 201 | } |
stanley1228 | 0:9b0f6c2eaea2 | 202 | |
stanley1228 | 0:9b0f6c2eaea2 | 203 | unsigned char lr=para[DEF_IX_PARA_GRP_LR]; |
stanley1228 | 0:9b0f6c2eaea2 | 204 | bool hold=para[DEF_IX_PARA_GRP_HOLD]; |
stanley1228 | 0:9b0f6c2eaea2 | 205 | int delay_ms=para[DEF_IX_PARA_GRP_DELAY_H]<<8 | para[DEF_IX_PARA_GRP_DELAY_L]; |
stanley1228 | 0:9b0f6c2eaea2 | 206 | |
stanley1228 | 0:9b0f6c2eaea2 | 207 | //pc.printf("lr=%d\n",lr); |
stanley1228 | 0:9b0f6c2eaea2 | 208 | //pc.printf("hole=%d\n",hold); |
stanley1228 | 0:9b0f6c2eaea2 | 209 | //pc.printf("delay_ms=%d\n",delay_ms); |
stanley1228 | 0:9b0f6c2eaea2 | 210 | |
stanley1228 | 0:9b0f6c2eaea2 | 211 | TB6612FNG_Gripper(lr,hold,delay_ms); |
stanley1228 | 0:9b0f6c2eaea2 | 212 | } |
stanley1228 | 0:9b0f6c2eaea2 | 213 | else if(cmd==DEF_CMD_FL) |
stanley1228 | 0:9b0f6c2eaea2 | 214 | { |
stanley1228 | 0:9b0f6c2eaea2 | 215 | para[0]=se3.getc(); |
stanley1228 | 0:9b0f6c2eaea2 | 216 | bool sw=(bool)para[0]; |
stanley1228 | 0:9b0f6c2eaea2 | 217 | |
stanley1228 | 0:9b0f6c2eaea2 | 218 | pin_footlifter=sw; |
stanley1228 | 0:9b0f6c2eaea2 | 219 | |
stanley1228 | 0:9b0f6c2eaea2 | 220 | //pc.printf("pin_footlifter=%d\n",sw); |
stanley1228 | 0:9b0f6c2eaea2 | 221 | |
stanley1228 | 0:9b0f6c2eaea2 | 222 | } |
stanley1228 | 0:9b0f6c2eaea2 | 223 | else if(cmd==DEF_CMD_SPINDLE) |
stanley1228 | 0:9b0f6c2eaea2 | 224 | { |
stanley1228 | 0:9b0f6c2eaea2 | 225 | para[0]=se3.getc(); |
stanley1228 | 0:9b0f6c2eaea2 | 226 | bool sw=(bool)para[0]; |
stanley1228 | 0:9b0f6c2eaea2 | 227 | |
stanley1228 | 0:9b0f6c2eaea2 | 228 | if(sw==true) |
stanley1228 | 0:9b0f6c2eaea2 | 229 | pin_spindle.write(1); //改為使用固定低速,所以直接給高低為準,但是腳位還是在類比腳位 原本的pin_spindle.write(0.378);//0.3780=1.25V minimum speed to sew |
stanley1228 | 0:9b0f6c2eaea2 | 230 | else |
stanley1228 | 0:9b0f6c2eaea2 | 231 | pin_spindle.write(0); |
stanley1228 | 0:9b0f6c2eaea2 | 232 | |
stanley1228 | 0:9b0f6c2eaea2 | 233 | //pc.printf("pin_spindle=%d\n",sw); |
stanley1228 | 0:9b0f6c2eaea2 | 234 | |
stanley1228 | 0:9b0f6c2eaea2 | 235 | } |
stanley1228 | 0:9b0f6c2eaea2 | 236 | else if(cmd==DEF_CMD_TRIMMER) |
stanley1228 | 0:9b0f6c2eaea2 | 237 | { |
stanley1228 | 0:9b0f6c2eaea2 | 238 | para[0]=se3.getc(); |
stanley1228 | 0:9b0f6c2eaea2 | 239 | bool sw=(bool)para[0]; |
stanley1228 | 0:9b0f6c2eaea2 | 240 | |
stanley1228 | 0:9b0f6c2eaea2 | 241 | pin_trimmer=sw; |
stanley1228 | 0:9b0f6c2eaea2 | 242 | |
stanley1228 | 0:9b0f6c2eaea2 | 243 | //pc.printf("pin_trimmer=%d\n",sw); |
stanley1228 | 0:9b0f6c2eaea2 | 244 | } |
stanley1228 | 0:9b0f6c2eaea2 | 245 | } |
stanley1228 | 0:9b0f6c2eaea2 | 246 | wait_ms(1); |
stanley1228 | 0:9b0f6c2eaea2 | 247 | } |
stanley1228 | 0:9b0f6c2eaea2 | 248 | } |