嵌入式记录2——Kame四足巡线机器人

这是本人嵌入式学习记录的第二期，仅作记录。

准备材料

Arduino Nano开发板（拓展版）

       Arduino Nano 是一款类似Arduino UNO的开发板。区别是Nano的尺寸更加小巧。Arduino Nano 是一款基于 ATmega328P的开发板。它可以直插面包板的 。Arduino Nano 与Arduino Uno十分类似。它与Uno的区别是Nano没有直流电压供电接口同时Nano通过Mini-B USB 接口与电脑连接。

主要技术参数

开发板原理图

黑色胶布

  黑色胶布用于搭建小车运行的“轨道”，选用黑色宽度18mm左右的即可。

循迹模块

  在此我们使用循迹模块TCRT5000，该模块体积小，灵敏度较高，还可以通过转动上面的电位器来调节检测范围。关于循迹控制，希望后期能加上PID进行高精度的控制，现在这里立个坑。

控制介绍

主控使用Arduino nano控制，不过这次每条腿添加了双线程控制，因此更加灵活，由于Arduino本身没有多线程，因此外置添加了一个Metro库，本质上是用定时器的millis函数写出来的，引用的开源项目可以参考：

Arduino 使用Metor库 简单实现多线程编程_ling3ye的博客-CSDN博客_arduino多线程编程

利用多线程实现对每条腿的控制延迟降低效果较好，同时通过每次旋转前反馈读取舵机角度，避免了大范围抖动旋转从而使舵机损坏，signalLegcontrol()函数针对每一条腿可以实现较好的控制

void signalLegcontrol(int value1,int value2,int cmd1,int cmd2){//单腿多线程控制
  if(servo1Metro.check()){
    servo1State =! servo1State;
    P1_angle = servo[cmd1].read();
    
    if(P1_angle < value1){
      for(P1_angle;P1_angle <= value1;P1_angle++){
        servo[cmd1].write(P1_angle);
        delayMicroseconds(1000);
      }       
    }
    else if(P1_angle > value1){
      for(P1_angle;P1_angle >= value1;P1_angle--){
        servo[cmd1].write(P1_angle);
        delayMicroseconds(1000);
      }  
    }
    else if(P1_angle = value1){
      servo[cmd1].write(P1_angle);
      delayMicroseconds(1000);        
    }
  }
  if(servo2Metro.check()){
    servo1State =! servo1State;
    P2_angle = servo[cmd2].read();

    if(P2_angle < value2){
      for(P2_angle;P2_angle <= value2;P2_angle++){
        servo[cmd2].write(P2_angle);
        delayMicroseconds(1000);
      }       
    }
    else if(P2_angle > value2){
      for(P2_angle;P2_angle >= value2;P2_angle--){
        servo[cmd2].write(P2_angle);
        delayMicroseconds(1000);
      }  
    }
    else if(P2_angle = value2){
      servo[cmd2].write(P2_angle);
      delayMicroseconds(1000);
    }  
  }      
}

利用该函数模块编写了手动模式和巡线模式两种，步态依旧采用‘X’型步态，同样暂时没有配合PID进行控制，但由于loop部分写得不是很好，仅用switch进行选择控制，因此要重新选择模式只能Reset，就很🐕，对于‘X’型步态规划，可以参考前一篇：

嵌入式学习记录（1）——四足蜘蛛机器人_Outの粉丝菌的博客-CSDN博客_四足蜘蛛机器人步态最终效果如下，由于使用的步态对于循迹模式不是很友好，因此后期必定需要改进

最终代码效果

#include <Servo.h>
#include <Metro.h>
#define tanglemax 175
#define tanglemin 5

#define Forward 0
#define TurnLeft 1
#define TurnRight 2
#define TurnBack 3
#define STOP 4

Servo servo[8];
int servo_pin[8];//舵机引脚
int P1_angle = 0;//当前角度值
int P2_angle = 0;
int P_angle[7];

unsigned int trace[4]={10,11,12,13};
boolean servo1State = false;
boolean servo2State = false;

String Trace;
String Hand;

Metro servo1Metro = Metro(20);
Metro servo2Metro = Metro(5);

void setup(){
  Serial.begin(9600);
  servoInitilize();
  Serial.println("Initilized Over!!!");
}

void loop() {
  //Tracing();
  while(Serial.available()>0){
    Serial.println("Mode Choose!!!");
    Serial.println("Please choose your wanted mode!!!");
    char SCMD = Serial.read();
    //if(Serial.find("Trace")){
    switch(SCMD){
      case 'T'://TracingMode
        Serial.println("Tracing Mode!!!");
        while(1){
          Serial.println("Tracing Mode!!!");
          Tracing();
        }
        break;
      case 'H'://HandleMode
        Serial.println("Handel Mode!!!");
        while(1){
          char serialcmd = Serial.read();
          HandControl(serialcmd);
        }
        break;
      case 'V'://STOPMode
        Serial.println("STOP!!!");
        motorRun(STOP);
        break;        
    }
  } 
}

void servoInitilize(){//对机器人进行初始化操作
  for(int i = 0;i <= 8;i++){
    servo_pin[i] = i+2;
    servo[i].attach(servo_pin[i],500,2500);
    servo[i].write(90);
    delayMicroseconds(500);
  }
  servo[0].write(45);
  delayMicroseconds(500);
  servo[1].write(135);
  delayMicroseconds(500);
  servo[2].write(135);
  delayMicroseconds(500);
  servo[3].write(45);
  delayMicroseconds(500); 
  Serial.println("Initilization!!!");
}
void signalLegcontrol(int value1,int value2,int cmd1,int cmd2){//单腿多线程控制
  if(servo1Metro.check()){
    servo1State =! servo1State;
    P1_angle = servo[cmd1].read();
    
    if(P1_angle < value1){
      for(P1_angle;P1_angle <= value1;P1_angle++){
        servo[cmd1].write(P1_angle);
        delayMicroseconds(1000);
      }       
    }
    else if(P1_angle > value1){
      for(P1_angle;P1_angle >= value1;P1_angle--){
        servo[cmd1].write(P1_angle);
        delayMicroseconds(1000);
      }  
    }
    else if(P1_angle = value1){
      servo[cmd1].write(P1_angle);
      delayMicroseconds(1000);        
    }
  }
  if(servo2Metro.check()){
    servo1State =! servo1State;
    P2_angle = servo[cmd2].read();

    if(P2_angle < value2){
      for(P2_angle;P2_angle <= value2;P2_angle++){
        servo[cmd2].write(P2_angle);
        delayMicroseconds(1000);
      }       
    }
    else if(P2_angle > value2){
      for(P2_angle;P2_angle >= value2;P2_angle--){
        servo[cmd2].write(P2_angle);
        delayMicroseconds(1000);
      }  
    }
    else if(P2_angle = value2){
      servo[cmd2].write(P2_angle);
      delayMicroseconds(1000);
    }  
  }      
}

void motorRun(int cmd){
  switch(cmd){
      case Forward://前进
        Serial.println("FORWARD!");
        signalLegcontrol(15,45,0,4);
        delayMicroseconds(1000);
        signalLegcontrol(15,90,0,4);
        delayMicroseconds(1000);
        signalLegcontrol(75,135,3,7);
        delayMicroseconds(1000);
        signalLegcontrol(75,90,3,7);
        delayMicroseconds(1000);
        signalLegcontrol(45,90,0,4);
        delayMicroseconds(1000);
        signalLegcontrol(45,90,3,7);
        delayMicroseconds(1000);//回正
        signalLegcontrol(165,135,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(165,90,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(105,45,2,6);
        delayMicroseconds(1000);
        signalLegcontrol(105,90,2,6);
        delayMicroseconds(1000);
        signalLegcontrol(135,90,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(135,90,2,6);
        delayMicroseconds(1000);//回正
        break;
      case TurnLeft://左转
        Serial.println("TURNLEFT!");
        signalLegcontrol(15,45,0,4);
        delayMicroseconds(1000);
        signalLegcontrol(15,90,0,4);
        delayMicroseconds(1000);
        signalLegcontrol(15,135,3,7);
        delayMicroseconds(1000);
        signalLegcontrol(15,90,3,7);
        delayMicroseconds(1000);
        signalLegcontrol(105,45,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(105,90,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(105,135,2,6);
        delayMicroseconds(1000);
        signalLegcontrol(105,90,2,6);
        delayMicroseconds(1000);
        signalLegcontrol(45,90,0,4);//回正
        delayMicroseconds(1000);
        signalLegcontrol(45,90,3,7);
        delayMicroseconds(1000);
        signalLegcontrol(135,90,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(135,90,2,6);
        delayMicroseconds(1000);        
        break;
      case TurnRight://右转
        Serial.println("TURNRIGHT!");
        signalLegcontrol(75,45,0,4);
        delayMicroseconds(1000);
        signalLegcontrol(75,90,0,4);
        delayMicroseconds(1000);
        signalLegcontrol(75,135,3,7);
        delayMicroseconds(1000);
        signalLegcontrol(75,90,3,7);
        delayMicroseconds(1000);
        signalLegcontrol(165,45,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(165,90,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(165,135,2,6);
        delayMicroseconds(1000);
        signalLegcontrol(165,90,2,6);
        delayMicroseconds(1000);
        signalLegcontrol(45,90,0,4);//回正
        delayMicroseconds(1000);
        signalLegcontrol(45,90,3,7);
        delayMicroseconds(1000);
        signalLegcontrol(135,90,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(135,90,2,6);
        delayMicroseconds(1000); 
        break;
      case TurnBack:
        Serial.println("TURNBACK!");
        signalLegcontrol(75,45,0,4);
        delayMicroseconds(1000);
        signalLegcontrol(75,90,0,4);
        delayMicroseconds(1000);
        signalLegcontrol(15,135,3,7);
        delayMicroseconds(1000);
        signalLegcontrol(15,90,3,7);
        delayMicroseconds(1000);
        signalLegcontrol(45,90,0,4);
        delayMicroseconds(1000);
        signalLegcontrol(45,90,3,7);
        delayMicroseconds(1000);//回正
        signalLegcontrol(105,135,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(105,90,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(165,45,2,6);
        delayMicroseconds(1000);
        signalLegcontrol(165,90,2,6);
        delayMicroseconds(1000);
        signalLegcontrol(135,90,1,5);
        delayMicroseconds(1000);
        signalLegcontrol(135,90,2,6);
        delayMicroseconds(1000);//回正
        break;
      case STOP:
        Serial.println("STOP!");
        //servoInitilize();//后续改进
        for(int Snum = 0;Snum <= 7;Snum++){
          P_angle[Snum] = servo[Snum].read();
          servo[Snum].write(P_angle[Snum]);
          delayMicroseconds(1000);
        }
        break;                                                  
    }    
}

void Tracing(){//循迹模式
  int data[4];
  data[0] = digitalRead(10);
  data[1] = digitalRead(11);
  data[2] = digitalRead(12);
  data[3] = digitalRead(13);

  if(!data[0] && !data[1] && !data[2] && !data[3])  //左右都没有检测到黑线
  {
    motorRun(Forward);
  }

  if(data[0] || data[1])  //右边检测到黑线
  {
    motorRun(TurnRight);
  }

  if(data[2] || data[3])  //左边检测到黑线
  {
    motorRun(TurnLeft);
  }

  if(data[0] && data[3])  //左右都检测到黑线是停止
  {
    motorRun(STOP);
    while(1);
  }
  
  Serial.print(data[0]);
  Serial.print("---");
  Serial.print(data[1]);
  Serial.print("---");
  Serial.print(data[2]);
  Serial.print("---");
  Serial.println(data[3]);  
}

void HandControl(char serialcmd){//手动控制模式
  switch(serialcmd){
    case 'F':
      Serial.println("Please enter Your Wnated Pases!!!:");
      motorRun(Forward);
      break;
    case 'L':
      motorRun(TurnLeft);
      break;
    case 'R':
      motorRun(TurnRight);
      break;
    case 'B':
      Serial.println("Please enter Your Wnated Pases!!!:");
      motorRun(TurnBack);    
      break;
     case 'S':
      motorRun(STOP);
      break;    
  }
}