For Sewing Robot use

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
stanley1228
Date:
Fri Jul 13 06:30:03 2018 +0000
Commit message:
For sewing robot use

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
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);    
+    }
+}
diff -r 000000000000 -r 9b0f6c2eaea2 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Jul 13 06:30:03 2018 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/fb8e0ae1cceb
\ No newline at end of file