chunye098 发表于 2019-11-29 20:27

四足蜘蛛机器人制作求救

本帖最后由 chunye098 于 2019-11-29 20:29 编辑

用了两个星期的下班时间完成了基本动作,可就是升级到蓝牙控制就是没有反应,手机能收到开机反馈,可手机发送命令就是没有反应,试过串口监视测试也没有问题,请大神帮助解决!




/*项目名称:遥控蜘蛛机器人
*作者:小川子,许康元
*日期:2018/09/30
*参考:Sunfounder设计的爬行机器人
*概述:通过组装蜘蛛机器人,感受Arduino的控制魅力,与单片机的强大。通过实践感受到自己能力的欠缺之处。
*---该项目前期是用Arduino UNO进行制作的,但是由于UNO 太大了,于是在项目完成之后改用 Nano
*   进行重新组装
*---在安装机器人没有问题后再进行本程序的调试
*---该程序动作函数参考Sunfounder设计的爬行机器人的演示代码
*---请确保正确安装了库文件
*/

//库函数----------------------------------------------------------------------
#include <Servo.h>    //舵机控制库
#include <FlexiTimer2.h>//设置定时器去同时控制多个舵机
#include <SerialCommand.h> //处理串行命令的库
SerialCommand SCmd;
// C 0 1: stand
// C 0 0: sit
// C 1 x: forward x step
// C 2 x: back x step
// C 3 x: right turn x step
// C 4 x: left turn x step
// C 5 x: hand shake x times
// C 6 x: hand wave x times
#define C_STAND_SIT    0
#define C_FORWARD      1
#define C_BACKWARD   2
#define C_LEFT         3
#define C_RIGHT      4
#define C_SHAKE      5
#define C_WAVE         6
#define S_STRAIGHT   1
#define S_BACK         2

//舵机对象--------4条腿 每条腿3个舵机 一共12个舵机------------------------------
Servo servo;
//设置信号输出引脚
const int servo_pin = { {2, 3, 4}, {5, 6, 7}, {8, 9, 10}, {11, 12, 13} };
/* 机器的尺寸和大小 ---------------------------------------------------------*/
const float length_a = 55;
const float length_b = 77.5;
const float length_c = 27.5;
const float length_side = 71;
const float z_absolute = -28;
/* 运动常量
----------------------------------------------------*/
const float z_default = -40, z_up = 90, z_boot = z_absolute;
const float x_default = 62, x_offset = 0;
const float y_start = 0, y_step = 40;
const float y_default = x_default;
intIn1 = A1;
intIn2 = A2;
intIn3 = A3;
intIn4 = A4;
intEnA = A0;
intEnB = A5;
int Check_angle=1;
int straiht_angle=180;
/* 运动变量
----------------------------------------------------*/
volatile float site_now;    //每只脚到末端的实时距离
volatile float site_expect; //预计每只脚到末端的实时距离
float temp_speed;   //每个轴的速度注意:需要在每次运动前重新计算
float speed_multiple = 1; //动作速度执行倍数
const float spot_turn_speed = 4;//转动速度
const float leg_move_speed = 8;//每条腿的移动速度
const float body_move_speed = 3; //身体移动速度
const float stand_seat_speed = 1; //站位速度
volatile int rest_counter;      //+1/0.02s, 自动停机时间
//函数传递时用的参数
const float KEEP = 255;
float move_speed=leg_move_speed;   //动作速度
//π值
const float pi = 3.1415926;
/* 转动常量
--------------------------------------------------------*/
//临时长度
const float temp_a = sqrt(pow(2 * x_default + length_side, 2) + pow(y_step, 2));
const float temp_b = 2 * (y_start + y_step) + length_side;
const float temp_c = sqrt(pow(2 * x_default + length_side, 2) + pow(2 * y_start + y_step + length_side, 2));
const float temp_alpha = acos((pow(temp_a, 2) + pow(temp_b, 2) - pow(temp_c, 2)) / 2 / temp_a / temp_b);
//site for turn
const float turn_x1 = (temp_a - length_side) / 2;
const float turn_y1 = y_start + y_step / 2;
const float turn_x0 = turn_x1 - temp_b * cos(temp_alpha);
const float turn_y0 = temp_b * sin(temp_alpha) - turn_y1 - length_side;
/* ---------------------------------------------------------------------------*/


//初始化函数
void setup()
{
//启动串口
Serial.begin(9600);
Serial.println("Robot Starts Initialization");
SCmd.addCommand("c", control_model);
SCmd.addCommand("d",speed_set);
//SCmd.addCommand("O",obstacle_model);
//SCmd.addCommand("F",follow_model);
SCmd.addCommand("s",straight_model);
SCmd.setDefaultHandler(unrecognized);
//初始4条腿的初始大小
set_site(0, x_default - x_offset, y_start + y_step,z_boot);
set_site(1, x_default - x_offset, y_start + y_step,z_boot);
set_site(2, x_default + x_offset, y_start,z_boot);
set_site(3, x_default + x_offset, y_start,z_boot);
for (int i = 0; i < 4; i++)
{
    for (int j = 0; j < 3; j++)
    {
      site_now = site_expect;
    }
}
//启动舵机控制服务(设置定时器)
FlexiTimer2::set(20, servo_service);
FlexiTimer2::start(); //开始定时器
Serial.println("Servo service strated");
//初始化舵机
servo_attach();//设置舵机接口函数
Serial.println("Servos Initialized");
Serial.println("Robot initialization Complete");
//直流电机初始化
straight_attach();
Serial.println("Straight Initialized");
}

void servo_attach(void)   //设置舵机连接串口
{
for (int i = 0; i < 4; i++)//4条腿
{
    for (int j = 0; j < 3; j++)//每条腿3个舵机
    {
      servo.attach(servo_pin);//设定舵机的接口
      delay(100);//等待100毫秒
    }
}
}

void servo_detach(void) //舵机分离串口
{
for (int i = 0; i < 4; i++)//4条腿
{
    for (int j = 0; j < 3; j++) //每条腿3个舵机
    {
      servo.detach();//使舵机与其接口分离
      delay(100);//等待100毫秒
    }
}
}

//主循环函数接受外部数据
void loop(){
SCmd.readSerial();
}

//检测舵机是否达到直流电机运动角度
int check_angel(void){
if(straiht_angle==0)
    return 1;
else
    return 0;
}
//设置直流电机运动四足角度
void set_angle(void){
set_site(0, x_default + x_offset, y_start , 20);
set_site(1, x_default + x_offset, y_start , 20);
set_site(2, x_default + x_offset, y_start , 20);
set_site(3, x_default + x_offset, y_start , 20);
for (int i = 0; i < 4; i++)
{
    for (int j = 0; j < 3; j++)
    {
      site_now = site_expect;
    }
}
straiht_angle=0;
}
//设置运动速率
void speed_set(void){
char *arg;
int set_speed,no_do;
Serial.println("Action:");
arg = SCmd.next();
set_speed= atoi(arg);
speed_multiple=set_speed;
arg = SCmd.next();
no_do = atoi(arg);
}
//串口库函数的默认错误调用函数
void unrecognized(const char *command) {
Serial.println("What?");
}

//设置直流电机接口
void straight_attach(void){
pinMode(In1,OUTPUT);
pinMode(In2,OUTPUT);
pinMode(In3,OUTPUT);
pinMode(In4,OUTPUT);
}

//直行模式
void straight_model(void){
char *arg;
int action_model,no_do;
Serial.println("Action:");
arg = SCmd.next();
action_model= atoi(arg);
arg = SCmd.next();
no_do = atoi(arg);
switch(action_model){
    case S_STRAIGHT:
          Serial.println("Straight Format");
          analogWrite(EnA,255);
          analogWrite(EnB,255);
          sit();
          if(!check_angel()){
            set_angle();
          }
          s_straight();
          break;
    case S_BACK:
          Serial.println("Straight Back");
          analogWrite(EnA,255);
          analogWrite(EnB,255);
          sit();
          if(!check_angel()){
            set_angle();
          }
          s_back();
          break;
    default:
          Serial.println("Undefine");
          break;
}
}

//直流电机前进
void s_straight(void){
digitalWrite(In1,HIGH);
digitalWrite(In2,LOW);
digitalWrite(In3,LOW);
digitalWrite(In4,HIGH);
}
//直流电机退后
void s_back(void){
digitalWrite(In1,LOW);
digitalWrite(In2,HIGH);
digitalWrite(In3,HIGH);
digitalWrite(In4,LOW);
}

//直流电机制动
void s_stop(void){
straiht_angle=180;
digitalWrite(In1,LOW);
digitalWrite(In2,LOW);
digitalWrite(In3,LOW);
digitalWrite(In4,LOW);
}


//循迹模式
void follow_model(void){

}
//避障模式
void obstacle_model(void)
{

}

//控制模式
void control_model(void)
{
    char *arg;
    int action_mode, n_step; //动作模式,移动步数
    Serial.println("Action:");
    arg = SCmd.next();
    action_mode = atoi(arg);
    arg = SCmd.next();
    n_step = atoi(arg);

    switch (action_mode)
    {
      case C_FORWARD:
      Serial.println("Step forward");
      if (!is_stand()){
             stand();
      }
      s_stop();
      step_forward(n_step);
      break;
      case C_BACKWARD:
      Serial.println("Step back");
      if (!is_stand()){
            stand();
      }
      s_stop();
      step_back(n_step);
      break;
      case C_LEFT:
      Serial.println("Turn left");
      if (!is_stand())
          stand();
      s_stop();
      turn_left(n_step);
      break;
      case C_RIGHT:
      Serial.println("Turn right");
      if (!is_stand())
          stand();
      s_stop();
      turn_right(n_step);
      break;
      case C_STAND_SIT:
      Serial.println("1:up,0:dn");
      if (n_step){
          s_stop();
          stand();
      } else{
          s_stop();
          sit();
      }

      break;
      case C_SHAKE:
      Serial.println("Hand shake");
      s_stop();
      hand_shake(n_step);
      break;
      case C_WAVE:
      Serial.println("Hand wave");
      s_stop();
      hand_wave(n_step);
      break;
      default:
      Serial.println("Error");
      break;
    }
}


//是否站立
bool is_stand(void)
{
if (site_now == z_default)
    return true;
else
    return false;
}

//坐下
void sit(void)
{
move_speed = stand_seat_speed;
for (int leg = 0; leg < 4; leg++)
{
    set_site(leg, KEEP, KEEP, z_boot);
}
wait_all_reach();
}

//站立
void stand(void)
{
move_speed = stand_seat_speed;
for (int leg = 0; leg < 4; leg++)
{
    set_site(leg, KEEP, KEEP, z_default);
}
wait_all_reach();
}


//左转
void turn_left(unsigned int step)
{
move_speed = spot_turn_speed;
while (step-- > 0)
{
    if (site_now == y_start)
    {
      //leg 3&1 move
      set_site(3, x_default + x_offset, y_start, z_up);
      wait_all_reach();

      set_site(0, turn_x1 - x_offset, turn_y1, z_default);
      set_site(1, turn_x0 - x_offset, turn_y0, z_default);
      set_site(2, turn_x1 + x_offset, turn_y1, z_default);
      set_site(3, turn_x0 + x_offset, turn_y0, z_up);
      wait_all_reach();

      set_site(3, turn_x0 + x_offset, turn_y0, z_default);
      wait_all_reach();

      set_site(0, turn_x1 + x_offset, turn_y1, z_default);
      set_site(1, turn_x0 + x_offset, turn_y0, z_default);
      set_site(2, turn_x1 - x_offset, turn_y1, z_default);
      set_site(3, turn_x0 - x_offset, turn_y0, z_default);
      wait_all_reach();

      set_site(1, turn_x0 + x_offset, turn_y0, z_up);
      wait_all_reach();

      set_site(0, x_default + x_offset, y_start, z_default);
      set_site(1, x_default + x_offset, y_start, z_up);
      set_site(2, x_default - x_offset, y_start + y_step, z_default);
      set_site(3, x_default - x_offset, y_start + y_step, z_default);
      wait_all_reach();

      set_site(1, x_default + x_offset, y_start, z_default);
      wait_all_reach();
    }
    else
    {
      //leg 0&2 move
      set_site(0, x_default + x_offset, y_start, z_up);
      wait_all_reach();

      set_site(0, turn_x0 + x_offset, turn_y0, z_up);
      set_site(1, turn_x1 + x_offset, turn_y1, z_default);
      set_site(2, turn_x0 - x_offset, turn_y0, z_default);
      set_site(3, turn_x1 - x_offset, turn_y1, z_default);
      wait_all_reach();

      set_site(0, turn_x0 + x_offset, turn_y0, z_default);
      wait_all_reach();

      set_site(0, turn_x0 - x_offset, turn_y0, z_default);
      set_site(1, turn_x1 - x_offset, turn_y1, z_default);
      set_site(2, turn_x0 + x_offset, turn_y0, z_default);
      set_site(3, turn_x1 + x_offset, turn_y1, z_default);
      wait_all_reach();

      set_site(2, turn_x0 + x_offset, turn_y0, z_up);
      wait_all_reach();

      set_site(0, x_default - x_offset, y_start + y_step, z_default);
      set_site(1, x_default - x_offset, y_start + y_step, z_default);
      set_site(2, x_default + x_offset, y_start, z_up);
      set_site(3, x_default + x_offset, y_start, z_default);
      wait_all_reach();

      set_site(2, x_default + x_offset, y_start, z_default);
      wait_all_reach();
    }
}
}

//- 右转
void turn_right(unsigned int step)
{
move_speed = spot_turn_speed;
while (step-- > 0)
{
    if (site_now == y_start)
    {
      //leg 2&0 move
      set_site(2, x_default + x_offset, y_start, z_up);
      wait_all_reach();

      set_site(0, turn_x0 - x_offset, turn_y0, z_default);
      set_site(1, turn_x1 - x_offset, turn_y1, z_default);
      set_site(2, turn_x0 + x_offset, turn_y0, z_up);
      set_site(3, turn_x1 + x_offset, turn_y1, z_default);
      wait_all_reach();

      set_site(2, turn_x0 + x_offset, turn_y0, z_default);
      wait_all_reach();

      set_site(0, turn_x0 + x_offset, turn_y0, z_default);
      set_site(1, turn_x1 + x_offset, turn_y1, z_default);
      set_site(2, turn_x0 - x_offset, turn_y0, z_default);
      set_site(3, turn_x1 - x_offset, turn_y1, z_default);
      wait_all_reach();

      set_site(0, turn_x0 + x_offset, turn_y0, z_up);
      wait_all_reach();

      set_site(0, x_default + x_offset, y_start, z_up);
      set_site(1, x_default + x_offset, y_start, z_default);
      set_site(2, x_default - x_offset, y_start + y_step, z_default);
      set_site(3, x_default - x_offset, y_start + y_step, z_default);
      wait_all_reach();

      set_site(0, x_default + x_offset, y_start, z_default);
      wait_all_reach();
    }
    else
    {
      //leg 1&3 move
      set_site(1, x_default + x_offset, y_start, z_up);
      wait_all_reach();

      set_site(0, turn_x1 + x_offset, turn_y1, z_default);
      set_site(1, turn_x0 + x_offset, turn_y0, z_up);
      set_site(2, turn_x1 - x_offset, turn_y1, z_default);
      set_site(3, turn_x0 - x_offset, turn_y0, z_default);
      wait_all_reach();

      set_site(1, turn_x0 + x_offset, turn_y0, z_default);
      wait_all_reach();

      set_site(0, turn_x1 - x_offset, turn_y1, z_default);
      set_site(1, turn_x0 - x_offset, turn_y0, z_default);
      set_site(2, turn_x1 + x_offset, turn_y1, z_default);
      set_site(3, turn_x0 + x_offset, turn_y0, z_default);
      wait_all_reach();

      set_site(3, turn_x0 + x_offset, turn_y0, z_up);
      wait_all_reach();

      set_site(0, x_default - x_offset, y_start + y_step, z_default);
      set_site(1, x_default - x_offset, y_start + y_step, z_default);
      set_site(2, x_default + x_offset, y_start, z_default);
      set_site(3, x_default + x_offset, y_start, z_up);
      wait_all_reach();

      set_site(3, x_default + x_offset, y_start, z_default);
      wait_all_reach();
    }
}
}

//前进
void step_forward(unsigned int step)
{
move_speed = leg_move_speed;
while (step-- > 0)
{
    if (site_now == y_start)
    {
      //leg 2&1 move
      set_site(2, x_default + x_offset, y_start, z_up);
      wait_all_reach();
      set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up);
      wait_all_reach();
      set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default);
      wait_all_reach();

      move_speed = body_move_speed;

      set_site(0, x_default + x_offset, y_start, z_default);
      set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default);
      set_site(2, x_default - x_offset, y_start + y_step, z_default);
      set_site(3, x_default - x_offset, y_start + y_step, z_default);
      wait_all_reach();

      move_speed = leg_move_speed;

      set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up);
      wait_all_reach();
      set_site(1, x_default + x_offset, y_start, z_up);
      wait_all_reach();
      set_site(1, x_default + x_offset, y_start, z_default);
      wait_all_reach();
    }
    else
    {
      //leg 0&3 move
      set_site(0, x_default + x_offset, y_start, z_up);
      wait_all_reach();
      set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up);
      wait_all_reach();
      set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default);
      wait_all_reach();

      move_speed = body_move_speed;

      set_site(0, x_default - x_offset, y_start + y_step, z_default);
      set_site(1, x_default - x_offset, y_start + y_step, z_default);
      set_site(2, x_default + x_offset, y_start, z_default);
      set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default);
      wait_all_reach();

      move_speed = leg_move_speed;

      set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up);
      wait_all_reach();
      set_site(3, x_default + x_offset, y_start, z_up);
      wait_all_reach();
      set_site(3, x_default + x_offset, y_start, z_default);
      wait_all_reach();
    }
}
}

//后退
void step_back(unsigned int step)
{
move_speed = leg_move_speed;
while (step-- > 0)
{
    if (site_now == y_start)
    {
      //leg 3&0 move
      set_site(3, x_default + x_offset, y_start, z_up);
      wait_all_reach();
      set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up);
      wait_all_reach();
      set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default);
      wait_all_reach();

      move_speed = body_move_speed;

      set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default);
      set_site(1, x_default + x_offset, y_start, z_default);
      set_site(2, x_default - x_offset, y_start + y_step, z_default);
      set_site(3, x_default - x_offset, y_start + y_step, z_default);
      wait_all_reach();

      move_speed = leg_move_speed;

      set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up);
      wait_all_reach();
      set_site(0, x_default + x_offset, y_start, z_up);
      wait_all_reach();
      set_site(0, x_default + x_offset, y_start, z_default);
      wait_all_reach();
    }
    else
    {
      //leg 1&2 move
      set_site(1, x_default + x_offset, y_start, z_up);
      wait_all_reach();
      set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up);
      wait_all_reach();
      set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default);
      wait_all_reach();

      move_speed = body_move_speed;

      set_site(0, x_default - x_offset, y_start + y_step, z_default);
      set_site(1, x_default - x_offset, y_start + y_step, z_default);
      set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default);
      set_site(3, x_default + x_offset, y_start, z_default);
      wait_all_reach();

      move_speed = leg_move_speed;

      set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up);
      wait_all_reach();
      set_site(2, x_default + x_offset, y_start, z_up);
      wait_all_reach();
      set_site(2, x_default + x_offset, y_start, z_default);
      wait_all_reach();
    }
}
}

//身体左倾
void body_left(int i)
{
set_site(0, site_now + i, KEEP, KEEP);
set_site(1, site_now + i, KEEP, KEEP);
set_site(2, site_now - i, KEEP, KEEP);
set_site(3, site_now - i, KEEP, KEEP);
wait_all_reach();
}

//身体右倾
void body_right(int i)
{
set_site(0, site_now - i, KEEP, KEEP);
set_site(1, site_now - i, KEEP, KEEP);
set_site(2, site_now + i, KEEP, KEEP);
set_site(3, site_now + i, KEEP, KEEP);
wait_all_reach();
}

//摇手
void hand_wave(int i)
{
float x_tmp;
float y_tmp;
float z_tmp;
move_speed = 1;
if (site_now == y_start)
{
    body_right(15);
    x_tmp = site_now;
    y_tmp = site_now;
    z_tmp = site_now;
    move_speed = body_move_speed;
    for (int j = 0; j < i; j++)
    {
      set_site(2, turn_x1, turn_y1, 50);
      wait_all_reach();
      set_site(2, turn_x0, turn_y0, 50);
      wait_all_reach();
    }
    set_site(2, x_tmp, y_tmp, z_tmp);
    wait_all_reach();
    move_speed = 1;
    body_left(15);
}
else
{
    body_left(15);
    x_tmp = site_now;
    y_tmp = site_now;
    z_tmp = site_now;
    move_speed = body_move_speed;
    for (int j = 0; j < i; j++)
    {
      set_site(0, turn_x1, turn_y1, 50);
      wait_all_reach();
      set_site(0, turn_x0, turn_y0, 50);
      wait_all_reach();
    }
    set_site(0, x_tmp, y_tmp, z_tmp);
    wait_all_reach();
    move_speed = 1;
    body_right(15);
}
}

//招手
void hand_shake(int i)
{
float x_tmp;
float y_tmp;
float z_tmp;
move_speed = 1;
if (site_now == y_start)
{
    body_right(15);
    x_tmp = site_now;
    y_tmp = site_now;
    z_tmp = site_now;
    move_speed = body_move_speed;
    for (int j = 0; j < i; j++)
    {
      set_site(2, x_default - 30, y_start + 2 * y_step, 55);
      wait_all_reach();
      set_site(2, x_default - 30, y_start + 2 * y_step, 10);
      wait_all_reach();
    }
    set_site(2, x_tmp, y_tmp, z_tmp);
    wait_all_reach();
    move_speed = 1;
    body_left(15);
}
else
{
    body_left(15);
    x_tmp = site_now;
    y_tmp = site_now;
    z_tmp = site_now;
    move_speed = body_move_speed;
    for (int j = 0; j < i; j++)
    {
      set_site(0, x_default - 30, y_start + 2 * y_step, 55);
      wait_all_reach();
      set_site(0, x_default - 30, y_start + 2 * y_step, 10);
      wait_all_reach();
    }
    set_site(0, x_tmp, y_tmp, z_tmp);
    wait_all_reach();
    move_speed = 1;
    body_right(15);
}
}



/*
- 舵机服务/定时器中断功能/50Hz
- 当设置site expected时,这个函数会移动到目标直线
- 在设置expect之前,应该设置temp_speed,确保
    直线移动,决定移动速度。
   ---------------------------------------------------------------------------*/
void servo_service(void)
{
sei();
static float alpha, beta, gamma;

for (int i = 0; i < 4; i++)
{
    for (int j = 0; j < 3; j++)
    {
      if (abs(site_now - site_expect) >= abs(temp_speed))
      site_now += temp_speed;
      else
      site_now = site_expect;
    }

    cartesian_to_polar(alpha, beta, gamma, site_now, site_now, site_now);
    polar_to_servo(i, alpha, beta, gamma);
}

rest_counter++;
}

void set_site(int leg, float x, float y, float z)//设置某一条腿的最终坐标
{
float length_x = 0, length_y = 0, length_z = 0;//初始化float类型变量length_x,length_y,length_z

if (x != KEEP)//假若x轴不是保持状态
    length_x = x - site_now;//计算x轴长度
if (y != KEEP)//假若y轴不是保持状态
    length_y = y - site_now;//计算y轴长度
if (z != KEEP)//假若z轴不是保持状态
    length_z = z - site_now;//计算z轴长度

float length = sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2));   //计算宗长度

temp_speed = length_x / length * move_speed * speed_multiple;//计算对应腿的舵机1移动速度
temp_speed = length_y / length * move_speed * speed_multiple;//计算对应腿的舵机2移动速度
temp_speed = length_z / length * move_speed * speed_multiple;//计算对应腿的舵机3移动速度

if (x != KEEP)//假若x轴不是保持状态,则设置目标角度
    site_expect = x;
if (y != KEEP)//假若y轴不是保持状态,则设置目标角度
    site_expect = y;
if (z != KEEP)//假若z轴不是保持状态,则设置目标角度
    site_expect = z;
}

/*单条腿部动作完成度检测
-----------------------------------------------------------------------*/
void wait_reach(int leg)//等待某条腿动作完成函数
{
while (1) //死循环
    if (site_now == site_expect)    //等待目标腿的 舵机 1达到目标角度
      if (site_now == site_expect)//等待目标腿的 舵机 2达到目标角度
      if (site_now == site_expect)//等待目标腿的 舵机 3达到目标角度
          break;//跳出循环
}

/*四条动作完成度检测
-----------------------------------------------------------------------*/
void wait_all_reach(void) //等待全部腿动作完成函数
{
for (int i = 0; i < 4; i++)   //依次等待4条腿动作完成
    wait_reach(i);
}

/*
- 从笛卡尔坐标系到极坐标转化
- 数学模型2/2
   ---------------------------------------------------------------------------*/
void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, volatile float x, volatile float y, volatile float z)
{
//calculate w-z degree
float v, w;
w = (x >= 0 ? 1 : -1) * (sqrt(pow(x, 2) + pow(y, 2)));
v = w - length_c;
alpha = atan2(z, v) + acos((pow(length_a, 2) - pow(length_b, 2) + pow(v, 2) + pow(z, 2)) / 2 / length_a / sqrt(pow(v, 2) + pow(z, 2)));
beta = acos((pow(length_a, 2) + pow(length_b, 2) - pow(v, 2) - pow(z, 2)) / 2 / length_a / length_b);
//calculate x-y-z degree
gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x);
//trans degree pi->180
alpha = alpha / pi * 180;
beta = beta / pi * 180;
gamma = gamma / pi * 180;
}

/*
- 用对应的极坐标控制舵机
- 数学模型与事实相吻合的情况下
- EEprom中存储的错误将被添加
   ---------------------------------------------------------------------------*/
void polar_to_servo(int leg, float alpha, float beta, float gamma)
{
if (leg == 0)
{
    alpha = 90 - alpha;
    beta = beta;
    gamma += 90;
}
else if (leg == 1)
{
    alpha += 90;
    beta = 180 - beta;
    gamma = 90 - gamma;
}
else if (leg == 2)
{
    alpha += 90;
    beta = 180 - beta;
    gamma = 90 - gamma;
}
else if (leg == 3)
{
    alpha = 90 - alpha;
    beta = beta;
    gamma += 90;
}
//Serial.println(alpha);
//Serial.println(beta);
//Serial.println(gamma);
servo.write(alpha);//设定对应腿上的舵机1旋转角度
servo.write(beta);   //设定对应腿上的舵机2旋转角度
servo.write(gamma);//设定舵机3旋转角度
}


清华大叔叔丶 发表于 2020-7-5 09:21

楼主,我最近也在做这个,我想问你12个舵机电源的问题是怎么解决的?:D

wangnan20 发表于 2020-7-27 15:51

老哥,解决了没?我也遇到这个问题,用电脑蓝牙连接都可以接收指令,就是手机不行E:\Downloads\捕获.PNG

wangnan20 发表于 2020-7-27 16:43

老哥,指令尾部要加'\r\n'

淘气的一丁 发表于 2022-4-28 20:32

清华大叔叔丶 发表于 2020-7-5 09:21
楼主,我最近也在做这个,我想问你12个舵机电源的问题是怎么解决的?

我也是卡到电源问题,2节18650还是带不动12个舵机

tuzixiao10 发表于 2022-6-10 10:53

大佬们 我也想做一个,有参考的教程吗?
页: [1]
查看完整版本: 四足蜘蛛机器人制作求救