qmaw的部落格

※因改造引起的Robi故障，本人及本站都不負賠償任，進行改造前需有小朋友出走的覺悟。

Robi用的伺服馬達，除了和其他伺服馬達一樣，可使用單向的PWM控制之外，
還可以使用雙向的TTL Packet控制，可以詢問馬達，目前的溫度，位移量等等，詳情請參考伺服馬達說明書

今天要作的測試是更改maximum CW/CCW Angle的值，對於伺服馬達轉動的影響。

CW=clockwise，順時鐘方向

CCW=counter clockwise，逆時鐘方向

Maximum Angle的值越大，可轉到的角度就越大，

為了方便DEMO，影片中加入了一個1602的LCD螢幕，使用I2C介面控制，詳細資訊請參考Cooper Maa的網誌。

[伺服馬達]S接Arduino Digital 3。電源就接+5V&GND。不曉得S、+、-三隻腳位的，可以看一下第三期的控制機板，上面有印。
[I2C LCD] SCL接Arduino Analog A5。SDA接Arduino Analog A4。電源就接+5V&GND。

程式碼如下：程式碼下載處https://box.nctu.edu.tw/public.php?service=files&t=a7731dfd6470594e51364d910c7055ef&download

// for RS308MD(Robi) singl servo test
#include 
#include  
#include <LiquidCrystal_I2C.h> 

SoftwareSerial robi = SoftwareSerial(2,3);    //RX , TX  pin D3 for tranfer DATA

byte set[]={0X01,0x00,0X1E,0X02,0X01};        //ID,Flag,Adress,Length,Count, 
byte Max_Torque[]={0x01,0x00,0x23,0x01};      //ID,Flag,Adress,Length,Count, 
byte Sum;
byte on[]={0x01,0x00,0x24,0x01,0x01,0x01,0x24};       //Torque Enable DATA
byte max_CW_Angle[]={0x01,0x00,0x08,0x02,0x01};
byte max_CCW_Angle[]={0x01,0x00,0x0A,0x02,0x01};
byte tx_Head_H=0XFA;
byte tx_Head_L=0XAF;
byte rx_Head_H=0XFD; //rx_Header's are reserved for checking rx pkt.
byte rx_Head_L=0XAF;
int i=0;
int Max_CW_Angle = 1500;
int Max_CCW_Angle = -1500;
byte CheckSum=0;

LiquidCrystal_I2C lcd(0x27,16,2); // set the LCD address to 0x27 for a 16 chars and 2 line display 

void setup() 
{
        
  robi.begin(115200);
  delay(500);
  lcd.init(); // initialize the lcd 
  // Print a message to the LCD. 
  lcd.backlight(); 
        
  set_Max_Torque(0x64);
  Torque_on();      
  move_s(0);
}

void loop()
{

  set_Max_CW_Angle(0x01, Max_CW_Angle-100*i);  
  set_Max_CCW_Angle(0x01, Max_CCW_Angle+100*i);  
  lcd.clear();      
  lcd.setCursor(0, 0);
  lcd.print("M CW Ang= ");
  lcd.print(Max_CW_Angle-100*i);
  
  lcd.setCursor(0, 1);
  lcd.print("M CCW Ang= ");
  lcd.print(Max_CCW_Angle+100*i);

  delay(500);
  move_s(1500);
  delay(5000);
  move_s(-1500);
  delay(5000);
  if (i<=12) 
    {i+=3;}
  else
    {i=0;}
}


void Servo_Send_Short_Pkt(byte Pkt_Head_H, byte Pkt_Head_L, byte Servo_ID, byte Flag, byte Address, byte Length,byte Count, int Data)
{
  byte Pkt_Data[Length];
        
  for (int i=0;i<Length;i++)
  {
    Pkt_Data[i]=lowByte(Data);
    Data=Data>>8;
  }

  robi.write(Pkt_Head_H);
  robi.write(Pkt_Head_L);
  robi.write(Servo_ID);        
  robi.write(Flag);
  robi.write(Address);
  robi.write(Length);
  robi.write(Count);
        //robi.write(Data);
  CheckSum= Servo_ID ^ Flag ^ Address ^ Length ^ Count;
  for (int i=0;i<Length;i++)
  {
    robi.write(Pkt_Data[i]);
    CheckSum=CheckSum^Pkt_Data[i];
   }
    robi.write(CheckSum);     
}


void Torque_on()
{
  Servo_Send_Short_Pkt(tx_Head_H,tx_Head_L,on[0],on[1],on[2],on[3],on[4],on[5]);
}

void move_s(int pos)
{
  Servo_Send_Short_Pkt(tx_Head_H,tx_Head_L,set[0],set[1],set[2],set[3],set[4],pos);
}

void set_Max_CW_Angle(byte ID, int angle)  
{
  Servo_Send_Short_Pkt(tx_Head_H,tx_Head_L, ID, max_CW_Angle[1],max_CW_Angle[2],max_CW_Angle[3],max_CW_Angle[4],angle);
}

void set_Max_CCW_Angle(byte ID, int angle)
{
  Servo_Send_Short_Pkt(tx_Head_H,tx_Head_L, ID, max_CCW_Angle[1],max_CCW_Angle[2],max_CCW_Angle[3],max_CCW_Angle[4],angle);
}

void set_Max_Torque(int pos)
{
  Servo_Send_Short_Pkt(tx_Head_H,tx_Head_L, Max_Torque[0], Max_Torque[1],Max_Torque[2],Max_Torque[3],Max_Torque[4],pos);
}