hsu han-lin
For Sewing Robot use
Diff: main.cpp
- Revision:
- 0:9b0f6c2eaea2
diff -r 000000000000 -r 9b0f6c2eaea2 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Jul 13 06:30:03 2018 +0000
@@ -0,0 +1,248 @@
+#include "mbed.h"
+
+//------------------------------------
+// Hyperterminal configuration
+// 9600 bauds, 8-bit data, no parity
+//------------------------------------
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+Serial se3(PC_10,PC_11); //left top of CN7,right top of CN7(MB1136)
+//DigitalOut myled(LED1);
+
+//==pin define==//
+//rotate motor
+DigitalOut pin_en(D2);
+DigitalOut pin_dir(D3);
+DigitalOut pin_step(D4);
+
+//gripper
+PwmOut pin_grp_pwmL(D10);
+DigitalOut pin_grpL_1(D8);
+DigitalOut pin_grpL_2(D9);
+PwmOut pin_grp_pwmR(D5);
+DigitalOut pin_grpR_1(D7);
+DigitalOut pin_grpR_2(D6);
+
+
+
+//foot lifter
+DigitalOut pin_footlifter(D12);
+
+//spindle
+AnalogOut pin_spindle(PA_4); //A2
+
+//trimmer
+DigitalOut pin_trimmer(D13);
+//================//
+//==Rotate Motor==//
+//================//
+#define DEF_STEP_M_EN false
+#define DEF_STEP_M_DIS true
+#define DEF_CLOCK_WISE true
+#define DEF_COUNTER_CLOCK_WISE false
+
+//腰身圓盤72格/圈 72格/360 = 1格/5deg = 0.2格/deg
+//馬達9格/圈 ->1格/40deg
+//EX:轉腰90deg -> 腰轉90*0.2=18格 -> 馬達轉18*40=720degree = 720/(1.8)*16=400*16=6400 step
+
+#define DEF_DEG_TO_STEPS (16/1.8)
+#define DEF_WAIST_GRID_PER_DEG 0.2f
+#define DEF_STEP_M_DEG_PER_GRID 40
+
+int RotateMotor(bool dir,float deg)
+{
+ int steps=deg*DEF_WAIST_GRID_PER_DEG*DEF_STEP_M_DEG_PER_GRID*DEF_DEG_TO_STEPS;
+
+ //pc.printf("steps=%d\n",steps);
+
+ pin_dir=dir;
+
+ pin_en=DEF_STEP_M_EN;
+
+ for(int i=0;i<steps;i++)
+ {
+ pin_step=true;
+ wait_ms(1);
+ pin_step=false;
+ wait_ms(1);
+ }
+
+
+ pin_en=DEF_STEP_M_DIS;
+ return 0;
+}
+
+//============//
+//==Gripper==//
+//============//
+#define DEF_RELEASE 0
+#define DEF_HOLD 1
+#define DEF_RIGHT_HAND 1
+#define DEF_LEFT_HAND 2
+
+void initial_TB6612FNG_Gripper()
+{
+ // Set PWM
+ pin_grp_pwmL.period_ms(2);
+ pin_grp_pwmL.write(0.5);
+
+ pin_grp_pwmR.period_ms(2);
+ pin_grp_pwmR.write(0.5);
+}
+void TB6612FNG_Gripper(int rh,bool hold,int delay_ms)
+{
+ if(rh==DEF_RIGHT_HAND)
+ {
+ if(hold==true)
+ {
+ pin_grpR_1=1;
+ pin_grpR_2=0;
+ wait_ms(delay_ms);
+ }
+ else
+ {
+ pin_grpR_1=0;
+ pin_grpR_2=1;
+ wait_ms(delay_ms);
+ }
+ pin_grpR_1=0;
+ pin_grpR_2=0;
+
+ }
+ else if(rh==DEF_LEFT_HAND)
+ {
+ if(hold==true)
+ {
+ pin_grpL_1=1;
+ pin_grpL_2=0;
+ wait_ms(delay_ms);
+ }
+ else
+ {
+ pin_grpL_1=0;
+ pin_grpL_2=1;
+ wait_ms(delay_ms);
+ }
+ pin_grpL_1=0;
+ pin_grpL_2=0;
+ }
+}
+
+
+//====================
+//communication struct
+//====================
+#define DEF_CMD_ROTATE_MOTOR 0xa1
+#define DEF_CMD_GRIP 0xa2
+#define DEF_CMD_FL 0xa3
+#define DEF_CMD_SPINDLE 0xa4
+#define DEF_CMD_TRIMMER 0xa5
+
+#define DEF_IX_PARA_DIR 0
+#define DEF_IX_PARA_DEG_HIGH 1
+#define DEF_IX_PARA_DEG_LOW 2
+#define DEF_ECHO_ROTATE_MOTOR_DONE 3
+
+#define DEF_IX_PARA_GRP_LR 0
+#define DEF_IX_PARA_GRP_HOLD 1
+#define DEF_IX_PARA_GRP_DELAY_H 2
+#define DEF_IX_PARA_GRP_DELAY_L 3
+
+
+
+
+
+int main()
+{
+ char para[4]={0};
+
+ initial_TB6612FNG_Gripper();
+
+ //pc.printf("start\n");
+
+ //TB6612FNG_Gripper(DEF_LEFT_HAND,false,500);
+ //RotateMotor(DEF_COUNTER_CLOCK_WISE,90);
+
+ while(1)
+ {
+ if(se3.readable())
+ {
+ char cmd;
+ cmd=se3.getc();
+
+
+ if(cmd==DEF_CMD_ROTATE_MOTOR)
+ {
+ //pc.printf("DEF_CMD_ROTATE_MOTOR\n");
+
+ for(int i=0;i<3;i++)
+ {
+ para[i]=se3.getc();
+ }
+ bool dir=(para[DEF_IX_PARA_DIR]==1)? DEF_CLOCK_WISE:DEF_COUNTER_CLOCK_WISE;
+ int deg=para[DEF_IX_PARA_DEG_HIGH]<<8 | para[DEF_IX_PARA_DEG_LOW];
+
+ //pc.printf("dir=%d\n",dir);
+ //pc.printf("deg=%d\n",deg);
+
+ RotateMotor(dir,deg);
+
+ //pc.printf("after RotateMotor\n");
+ //echo to master after rotate done
+ cmd=DEF_ECHO_ROTATE_MOTOR_DONE;
+ se3.putc(cmd);
+
+ }
+ else if(cmd==DEF_CMD_GRIP)
+ {
+ for(int i=0;i<4;i++)
+ {
+ para[i]=se3.getc();
+ }
+
+ unsigned char lr=para[DEF_IX_PARA_GRP_LR];
+ bool hold=para[DEF_IX_PARA_GRP_HOLD];
+ int delay_ms=para[DEF_IX_PARA_GRP_DELAY_H]<<8 | para[DEF_IX_PARA_GRP_DELAY_L];
+
+ //pc.printf("lr=%d\n",lr);
+ //pc.printf("hole=%d\n",hold);
+ //pc.printf("delay_ms=%d\n",delay_ms);
+
+ TB6612FNG_Gripper(lr,hold,delay_ms);
+ }
+ else if(cmd==DEF_CMD_FL)
+ {
+ para[0]=se3.getc();
+ bool sw=(bool)para[0];
+
+ pin_footlifter=sw;
+
+ //pc.printf("pin_footlifter=%d\n",sw);
+
+ }
+ else if(cmd==DEF_CMD_SPINDLE)
+ {
+ para[0]=se3.getc();
+ bool sw=(bool)para[0];
+
+ if(sw==true)
+ pin_spindle.write(1); //改為使用固定低速，所以直接給高低為準，但是腳位還是在類比腳位 原本的pin_spindle.write(0.378);//0.3780=1.25V minimum speed to sew
+ else
+ pin_spindle.write(0);
+
+ //pc.printf("pin_spindle=%d\n",sw);
+
+ }
+ else if(cmd==DEF_CMD_TRIMMER)
+ {
+ para[0]=se3.getc();
+ bool sw=(bool)para[0];
+
+ pin_trimmer=sw;
+
+ //pc.printf("pin_trimmer=%d\n",sw);
+ }
+ }
+ wait_ms(1);
+ }
+}