File content as of revision 0:9b0f6c2eaea2:

#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);    
    }
}