蜘蛛Robot upgrade v3.0 - Quadruped Robot upgrade v3.0

經過數次的實驗，腳的關節的結構是會直接影響robot的機體穩定，之前在設計Femur與Tibia的關節時覺得影響不大，結果機體做運動時會搖晃，
甚至發生servo的連杆都脫落。

本次升級著重在3(Coxa, Femur, Tibia)個關節強化，各關節在運動可以維持在軸心旋轉，並且確實不會左右晃動。這次升級體會機構件的設計必須非常紮實，不然software再怎麼改都無法讓機械腳移動到正確位置而難以開發gait步態。

《成果圖》

設計說明如下：

工具 & 材料：

3D繪圖: Sketch up

3D printer: Prusa i3

切片: Cura 14.09

材料: PLA 1.75mm

Arduino Pro mini x 1

DC-DC: 12v->5v/3A x 1
SG-90 servo x 12

Lithium battery 12v 

2mmx6mm screw x 14

因為Servo底端並無支撐的結構，所以我特地多了一片2mm的底座，長出支柱讓旋轉桿可以套入。

《Sketchup 3D design》

Coxa與Femur的連桿用2mm螺絲鎖緊，要記得連桿有方向性(鎖Servo連杆)。

這是用Cura把列印件排好，要印兩份。

這個也是要印兩份。

我有試著用Slic3r切片，結果慘不忍睹，每個洞都要銼刀磨，最麻煩是支撐柱，磨到手軟。
建議用Cure，切出來的孔非常精准，套上去很精確！

《3關節強化是重點》

組合後，哈哈，超級穩定！

電路圖(simple schematic)

IK source code：
呼叫 DO_IK() 前把要移動腳的座標In_Pos_X/Y/Z還有旋轉的座標設定好，就可以計算出該腳位3個關節角度，然後驅動servo進行運動，你會發現腳會靈活地變化，非常簡單！

void Do_IK_Init(void)
{
  float x, y, z;
  In_Pos_X = In_Pos_Y = In_Pos_Z = In_ROT_X = In_ROT_Y = In_ROT_Z = 0;

  x = cos(INIT_X * DEG_TO_RAD) * (COXA_LEN + FEMUR_LEN);
  y = sin(INIT_Z * DEG_TO_RAD) * (COXA_LEN + FEMUR_LEN);
  z = TIBIA_LEN;

  Leg_Init_Pos[RF].X = x;
  Leg_Init_Pos[RF].Y = y;
  Leg_Init_Pos[RF].Z = z;

  Leg_Init_Pos[RR].X = x;
  Leg_Init_Pos[RR].Y = -y;
  Leg_Init_Pos[RR].Z = z;

  Leg_Init_Pos[LF].X = -x;
  Leg_Init_Pos[LF].Y = y;
  Leg_Init_Pos[LF].Z = z;

  Leg_Init_Pos[LR].X = -x;
  Leg_Init_Pos[LR].Y = -y;
  Leg_Init_Pos[LR].Z = z;

  Leg_Total[RF].X = Leg_Init_Pos[RF].X + BODY_W / 2;
  Leg_Total[RF].Y = Leg_Init_Pos[RF].Y + BODY_L / 2;
  Leg_Total[RF].Z = TIBIA_LEN;
  Leg_Total[RR].X = Leg_Init_Pos[RR].X + BODY_W / 2;
  Leg_Total[RR].Y = Leg_Init_Pos[RR].Y - BODY_L / 2;
  Leg_Total[RR].Z = TIBIA_LEN;
  Leg_Total[LF].X = Leg_Init_Pos[LF].X - BODY_W / 2;
  Leg_Total[LF].Y = Leg_Init_Pos[LF].Y + BODY_L / 2;
  Leg_Total[LF].Z = TIBIA_LEN;
  Leg_Total[LR].X = Leg_Init_Pos[LR].X - BODY_W / 2;
  Leg_Total[LR].Y = Leg_Init_Pos[LR].Y - BODY_L / 2;
  Leg_Total[LR].Z = TIBIA_LEN;
}

void Do_IK(int leg)
{
  float Body_X, Body_Y, Body_Z, New_X, New_Y, New_Z, Total_X, Total_Y, Total_Z;
  float SIN_X, COS_X, SIN_Y, COS_Y, SIN_Z, COS_Z;

  // Body-IK
  SIN_X = sin(In_ROT_X * DEG_TO_RAD);
  COS_X = cos(In_ROT_X * DEG_TO_RAD);
  SIN_Y = sin(In_ROT_Y * DEG_TO_RAD);
  COS_Y = cos(In_ROT_Y * DEG_TO_RAD);
  SIN_Z = sin(In_ROT_Z * DEG_TO_RAD);
  COS_Z = cos(In_ROT_Z * DEG_TO_RAD);

  Total_X = Leg_Total[leg].X + In_Pos_X;
  Total_Y = Leg_Total[leg].Y + In_Pos_Y;
  Total_Z = Leg_Total[leg].Z;

  Body_X = (Total_X * COS_Y * COS_Z - Total_Y * COS_Y * SIN_Z + Total_Z * SIN_Y) - Total_X;
  Body_Y = (Total_X * COS_X * SIN_Z + Total_X * COS_Z * SIN_Y * SIN_X + Total_Y * COS_Z * COS_X - Total_Y * SIN_Z * SIN_Y * SIN_X - Total_Z * COS_Y * SIN_X) - Total_Y;
  Body_Z = (Total_X * SIN_X * SIN_Z - Total_X * COS_Z * SIN_Y * COS_X + Total_Y * COS_Z * SIN_X + Total_Y * SIN_Z * SIN_Y * COS_X + Total_Z * COS_Y * COS_X) - Total_Z;

  New_X = Body_X + In_Pos_X + Leg_Init_Pos[leg].X;
  New_Y = Body_Y + In_Pos_Y + Leg_Init_Pos[leg].Y;
  New_Z = Body_Z + In_Pos_Z + Leg_Init_Pos[leg].Z;

  Leg_Pos[leg].X = New_X * cos((PI / 4) + (PI / 2 * leg)) -  New_Y * sin((PI / 4) + (PI / 2 * leg));
  Leg_Pos[leg].Y = New_X * sin((PI / 4) + (PI / 2 * leg)) +  New_Y * cos((PI / 4) + (PI / 2 * leg));
  Leg_Pos[leg].Z = New_Z;


  Do_Leg_IK_Cal(leg);
}

/*
  leg: 0~3
*/
void Do_Leg_IK_Cal(int leg)
{
  float leg_len, HF, a1, a2, b1, T_angle, F_angle, C_angle;

  C_angle = 90 - atan2(Leg_Pos[leg].Y, Leg_Pos[leg].X) * RAD_TO_DEG;

  leg_len = cos(C_angle * DEG_TO_RAD) * Leg_Pos[leg].Y;
  //leg_len = sqrt(pow(Leg_Pos[leg].X, 2) + pow(Leg_Pos[leg].Y, 2));

  HF = sqrt(pow((leg_len - COXA_LEN), 2) + pow(Leg_Pos[leg].Z, 2));
  a1 = atan2((leg_len - COXA_LEN), Leg_Pos[leg].Z);
  a2 = acos((pow(TIBIA_LEN, 2) - pow(FEMUR_LEN, 2) - pow(HF, 2)) / (-2 * FEMUR_LEN * HF));
  F_angle = (a1 + a2) * RAD_TO_DEG - 90;
  b1 = acos((pow(HF, 2) - pow(TIBIA_LEN, 2) - pow(FEMUR_LEN, 2)) / (-2 * FEMUR_LEN * TIBIA_LEN));
  T_angle = 90 - b1 * RAD_TO_DEG;

  if (leg == 1 || leg == 3)
  {
    angle_req[leg][TIBIA] = 90 + T_angle;
    angle_req[leg][FEMUR] = 90 - F_angle;
  }
  else
  {
    angle_req[leg][TIBIA] = 90 - T_angle;
    angle_req[leg][FEMUR] = 90 + F_angle;
  }

  angle_req[leg][COXA] = 90 - C_angle;

}

《servo旋轉方向》