hsu han-lin / Mbed 2 deprecated RoboticArmIO

For Sewing Robot use

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }