File content as of revision 1:4f4b999b4939:

#include "mbed.h"
#include "RS405cb.h"

RS405cb::RS405cb(PinName tx,PinName rx,PinName permit):Serial(tx,rx),_permit(permit){
    baud(115200);
}

void RS405cb::Sho_Init(int center_vert,int center_hori,int pls15_vert,int pls15_hori,int mns15_vert,int mns15_hori){
    //TORQUE_ON(1);
    TORQUE_ON(2);
    TORQUE_ON(3);
    //TORQUE_ON(4);
    
    center_v = center_vert;
    center_h = center_hori;
    pls15_v = pls15_vert;
    pls15_h = pls15_hori;
    mns15_v = mns15_vert;
    mns15_h = mns15_hori;
}

void RS405cb::RSW1(unsigned char id,unsigned char flg1,unsigned char adr1,unsigned char len1,unsigned char cnt1,unsigned char data1){
    unsigned char sum;
    sum = id ^ flg1 ^ adr1 ^ len1 ^ cnt1 ^ data1;
    _permit = 1;
    putc(0xFA);
    putc(0xAF);
    putc(id);
    putc(flg1);
    putc(adr1);
    putc(len1);
    putc(cnt1);
    putc(data1);
    putc(sum);
    putc(0x00);
    //putc(0x00);
    while(!writeable()){}
    _permit = 0;
}

void RS405cb::RSW2(unsigned char id,unsigned char flg,unsigned char adr,unsigned char len,unsigned char cnt,unsigned char lowdata,unsigned char highdata){
    unsigned char sum;
    sum = id ^ flg ^ adr ^ len ^ cnt ^ lowdata ^ highdata;
    _permit = 1;
    putc(0xFA);
    putc(0xAF);
    putc(id);
    putc(flg);
    putc(adr);
    putc(len);
    putc(cnt);
    putc(lowdata);
    putc(highdata);
    putc(sum);
    putc(0x00);
    //putc(0x00);
    while(!writeable()){}
    _permit = 0;
}

void RS405cb::RSW0(unsigned char id,unsigned char flg,unsigned char adr,unsigned char len,unsigned char cnt){
    unsigned char sum;
    sum = id ^ flg ^ adr ^ len ^ cnt;
    _permit = 1;
    putc(0xFA);
    putc(0xAF);
    putc(id);
    putc(flg);
    putc(adr);
    putc(len);
    putc(cnt);
    putc(sum);
    putc(0x00);
    //putc(0x00);
    while(!writeable()){}
    _permit = 0;
}

void RS405cb::Rotate_Servo(unsigned char id,char RL,unsigned int ANGLE){
  if(RL == RIGHT){
      unsigned char hd1,ld1;
      hd1 = (ANGLE & 0xFF00) >> 8;
      ld1 = (ANGLE & 0x00FF);
      RSW2(id,0x00,0x1E,0x02,0x01,ld1,hd1);
  }
  else{
      unsigned char hd2,ld2;
      hd2 = ((0x10000 - ANGLE) & 0xFF00) >> 8;
      ld2 = ((0x10000 - ANGLE) & 0x00FF);
      RSW2(id,0x00,0x1E,0x02,0x01,ld2,hd2);
  }
}

void RS405cb::Rotate_Servo_Float(unsigned char id,float ANGLE){
    unsigned char hd,ld;
    if(ANGLE >=0){
        hd = ((int)(ANGLE*10.0) & 0x0000FF00) >> 8;
        ld = ((int)(ANGLE*10.0) & 0x000000FF);
    }
    else if(ANGLE <0){
        hd = ((0x100000000 + (int)(ANGLE*10.0))&0x0000FF00)>>8;
        ld = ((0x100000000 + (int)(ANGLE*10.0))&0x000000FF);
    }
    else{}
    RSW2(id,0x00,0x1E,0x02,0x01,ld,hd);
}

void RS405cb::Rotate_Servo_Float_Test(unsigned char id){
    while(1){
        TORQUE_ON(id);
        Rotate_Servo_Float(id,0.0);
        wait(1.0);
        Rotate_Servo_Float(id,50.0);
        wait(1.0);
        Rotate_Servo_Float(id,-20.0);
        wait(1.0);
    }
}

void RS405cb::Sho_Rotate_Servo(unsigned char id,int VH,float angle_of_attack){
    int center,pls15,mns15;
    if(VH == VERTICAL){
        center = center_v;
        pls15 = pls15_v;
        mns15 = mns15_v;
    }
    else{
        center = center_h;
        pls15 = pls15_h;
        mns15 = mns15_h;    
    }
    
    if(angle_of_attack >= 0){Rotate_Servo_Float(id,center + (pls15 - center)*(angle_of_attack / 15.0));}
    else{Rotate_Servo_Float(id,mns15 + (center - mns15)*((15.0 + angle_of_attack) / 15.0));}
}

void RS405cb::TORQUE_ON(unsigned char id){
  RSW1(id,0x00,0x24,0x01,0x01,0x01);
}

void RS405cb::ID_CHANGE(unsigned char oldid,unsigned char newid){
  RSW1(oldid,0x00,0x04,0x01,0x01,newid);
  wait(0.5);
  RSW0(newid,0x40,0xFF,0x00,0x00);
  wait(3.0);
  RSW0(newid,0x20,0xFF,0x00,0x00);
  while(1){
    Rotate_Servo_Float_Test(newid);
  }
}

void RS405cb::DECIDE_LIMIT_ANGLE(unsigned char id,unsigned int CWlimit,unsigned int CCWlimit){
  RSW2(id,0x00,0x08,0x02,0x01,(unsigned char)(CWlimit & 0x00FF),(unsigned char)((CWlimit & 0xFF00)>>8));
  RSW2(id,0x00,0x08,0x02,0x01,(unsigned char)((0x10000 - CCWlimit) & 0x00FF),(unsigned char)(((0x10000-CCWlimit) & 0xFF00)>>8));
  RSW0(id,0x40,0xFF,0x00,0x00);
  wait(2.0);
  RSW0(id,0x20,0xFF,0x00,0x00);
}

void RS405cb::REQUIRE_RETURN_PACKET(unsigned char id){
  //RSW1(id,0x09,0x24,0x01,0x01,0x01);
  RSW0(id,0x09,0x00,0x00,0x01);
}