【arduino】小车源码记录

#include <Servo.h>  
Servo servo_chassis;  //创建伺服对象来控制伺服,底盘
Servo servo_arm;      //臂
Servo servo_elbow;    //肘
Servo servo_hand;     //手爪
#define PIN_SERVO_CHASSIS 5     //底盘
#define PIN_SERVO_ARM 6         //臂
#define PIN_SERVO_ELBOW 9       //肘
#define PIN_SERVO_HAND 3       //手爪

void chassis_Clockwise();      //底盘顺时针转动
void chassis_Anticlockwise();  //底盘逆时针转动
void arm_Up();                 //臂上升
void arm_Down();               //臂下降
void elbow_Extend();           //肘展开
void elbow_Back();             //肘收回
void hand_Hold();              //手爪握紧
void hand_Release();           //手爪放松
void serial_control();         //由串口获取x_chassis,y_chassis值
void reset();                  //整个机械爪复位
void kinemati_analy();         //运动学分析
/*serial_control()的变量*/
//char inByte = 0;             //串口接收的数据
//int angle = 0;               //角度值
//String temp = "";            //临时字符变量，又或者说是缓存用的吧


float chassis_angle=0;      //底盘舵机角度
float arm_angle=0;          //臂舵机角度
float elbow_angle=0;        //肘舵机角度
float hand_angle=0;         //手爪舵机角度

float max_chassis_angle=180;     //底盘舵机复位角度
float min_chassis_angle=0;       //底盘舵机有效角度
float max_arm_angle=135;         //臂舵机复位角度
float min_arm_angle=70;          //臂舵机有效角度
float max_elbow_angle=159;       //肘舵机复位角度
float min_elbow_angle=23;         //肘舵机有效角度
float max_hand_angle=157;        //手爪舵复位机角度
float min_hand_angle=100;         //手爪舵机有效角度

float x_camera=0;           //小球对于摄像头x坐标
float y_camera=0;           //小球对于摄像头y坐标
float x_chassis=0;          //小球对于底盘x坐标
float y_chassis=0;          //小球对于底盘y坐标
float i=0;
float serial_control_factor=0;
 
float chassis_high=9;   //底座高度mm
float arm_long=13;      //手臂长度
float elbow_hand_long=16.5;   //手爪到肘长
float chassis_ball;   //底座到球距离
float radian_chassis; //底座转角角度
float chassis_angle_camera; //底座转角角度
float radian_arm=0;     //臂转角角度
float radian_elbow=0;   //肘转角角度


void setup() 
{ 
   servo_chassis.attach(PIN_SERVO_CHASSIS);  
   servo_arm.attach(PIN_SERVO_ARM);  
   servo_elbow.attach(PIN_SERVO_ELBOW);  
   servo_hand.attach(PIN_SERVO_HAND);  
   Serial.begin(9600);     //设置波特率
   Serial.println("输入x_chassis和y_chassis，并以逗号隔开，如12，23");
   while(Serial.read()>= 0){} //清除缓存
   i=1;
   reset();    


}  
  /**********************************函数集合****************************************************
      chassis_Clockwise();      //底盘顺时针转动
      chassis_Anticlockwise();  //底盘逆时针转动
      arm_Up();                 //臂上升
      arm_Down();               //臂下降
      elbow_Extend();           //肘展开
      elbow_Back();             //肘收回
      hand_Hold();              //手爪握紧
      hand_Release();           //手爪放松
      serial_control();         //由串口获取x_chassis,y_chassis值
      reset();                  //整个机械爪复位
  *********************************************************************************************/
  /********************************主函数****************************************************/
void loop()  
{ 

    while(i==1) //先松后尽
   //  serial_control();         //由串口获取x_chassis,y_chassis值
    {
      
    i=1;
  serial_control();   //由串口获取x_chassis,y_chassis值
  
   //delay(5000);
   // ;
   kinemati_analy();         //运动学分析
   i=1;
  }
} 
/************************************************************************************/



void kinemati_analy()         //运动学分析
{ 
  Serial.println("进入运动学分析");
  //while(i==1){

  //大齿轮50个，小齿轮17个，齿轮比17比50，小齿轮转180度，大齿轮转n=17*180/50=61.2     //61.2/2=30.6,   90-0.6=59.4/// 17/50=0.34
  //小齿轮角度== 50*radian_chassis/(17)+90；
  // *50/(17*180)
   
 //x_chassis=-10; 
 //  y_chassis=50;
   
   Serial.print("x_chassis=");
   Serial.println(x_chassis); 
   Serial.print("y_chassis=");
   Serial.println(y_chassis);
   
 /********************底座α角计算换算**************************/ 
  radian_chassis = atan(x_chassis/y_chassis)*180/3.14;   //atan得到弧度，换算为角度

   Serial.print("小球偏移中心线角度：radian_chassis=");
   Serial.println(radian_chassis);
   //chassis_angle_camera=90-radian_chassis*50/17;                      //可以是这个，继续转换成舵机的转动角度
   chassis_angle_camera=90-radian_chassis/0.34;                         //99-的作用是转换坐标轴，当radian_chassis为30.6时，角度=90-90=0；当radian_chassis为0时，90-0=90，处于中间
   Serial.print("舵机运行角度：chassis_angle_camera=");
   Serial.println(chassis_angle_camera);
   
  if( chassis_angle_camera>=0&&chassis_angle_camera<=180)    //判断是否在舵机运行范围
    {
      chassis_angle=servo_chassis.read();//读取度数，第一次是90
       Serial.print("在运行范围， 将转到小球偏移角度：");
       Serial.println(radian_chassis);
      for(;chassis_angle<chassis_angle_camera;chassis_angle++)//输入数和状态比较
      {
       servo_chassis.write(chassis_angle);
       delay(30);
      }
       for(;chassis_angle>chassis_angle_camera;chassis_angle--)//输入数和状态比较
      {
       servo_chassis.write(chassis_angle);
       delay(30);
      }
       }
     
     else{
      Serial.println("已超出范围");
      }

 }
 /******************手臂和肘部βθ角计算换算********************/
  // chassis_ball=sqrt(x_chassis^2+y_chassis^2);//底座到球距离
 //chassis_ball = arm_long * sin(radian_arm) + elbow_hand_long * cos(radian_elbow);
 //chassis_high = arm_long* cos(radian_arm) + elbow_hand_long * sin(radian_elbow);




void reset()                  //整个机械爪复位
{
  servo_chassis.write(max_chassis_angle/2);   //底盘
  servo_arm.write(max_arm_angle);           //臂
  servo_elbow.write(min_elbow_angle);       //肘
  servo_hand.write(max_hand_angle);        //手爪
  Serial.println("已进行机械爪复位");
  delay(2000);
}

void chassis_Clockwise()     //底盘顺时针转动
{
  while(i==1)
  {
    for(chassis_angle=min_chassis_angle;chassis_angle<max_chassis_angle;chassis_angle++)
   {
  servo_chassis.write(chassis_angle);  //设定舵机旋转角度的语句，可设定的角度范围是0°到180°。
   delay(30);   
   }

    Serial.print("底盘顺时针转动，此时角度为");
    Serial.println(chassis_angle);
    i=0;
   }
}


void chassis_Anticlockwise()   //底盘逆时针转动
{
  while(i==1)
  {
    for(chassis_angle=max_chassis_angle;chassis_angle>min_chassis_angle;chassis_angle--)
   {
   servo_chassis.write(chassis_angle);  //设定舵机旋转角度的语句，可设定的角度范围是0°到180°。
   delay(30);   
   }
   
    Serial.print("底盘顺时针转动，此时角度为");
    Serial.println(chassis_angle);
    i=0;
   }
}

void  arm_Up()                 //臂上升
{
  while(i==1)
  {
    for(arm_angle=min_arm_angle;arm_angle<max_arm_angle;arm_angle++)
   {
  servo_arm.write(arm_angle);  //设定舵机旋转角度的语句，可设定的角度范围是0°到180°。
   delay(40);   
   }

    Serial.print("臂上升，此时角度为");
    Serial.println(arm_angle);
    i=0;
   }
}

void  arm_Down()               //臂下降
{
  while(i==1)
  {
    for(arm_angle=max_arm_angle;arm_angle>min_arm_angle;arm_angle--)
   {
  servo_arm.write(arm_angle);  //设定舵机旋转角度的语句，可设定的角度范围是0°到180°。
   delay(40);   
   }
  
    Serial.print("臂上升，此时角度为");
    Serial.println(arm_angle);
    i=0;
   }
}
void elbow_Extend()           //肘展开
{
  while(i==1)
  {
    for(elbow_angle=min_elbow_angle;elbow_angle<max_elbow_angle;elbow_angle++)
   {
  servo_elbow.write(elbow_angle);  //设定舵机旋转角度的语句，可设定的角度范围是0°到180°。
   delay(30);   
   }

    Serial.print("肘展开，此时角度为");
    Serial.println(elbow_angle);
    i=0;
   }
}
void elbow_Back()             //肘收回
{
  while(i==1)
  {
    for(elbow_angle=max_elbow_angle;elbow_angle>min_elbow_angle;elbow_angle--)
   {
  servo_elbow.write(elbow_angle);  //设定舵机旋转角度的语句，可设定的角度范围是0°到180°。
   delay(30);   
   }
   
    Serial.print("肘展开，此时角度为");
    Serial.println(elbow_angle);
    i=0;
   }
}


void hand_Hold()  //手爪握紧
{
  while(i==1)
  {
    for(hand_angle=min_hand_angle;hand_angle<max_hand_angle;hand_angle++)
   {
   servo_hand.write(hand_angle);  //设定舵机旋转角度的语句，可设定的角度范围是0°到180°。
   delay(30);   
   }

    Serial.print("手爪握紧，此时角度为");
    Serial.println(hand_angle);
    i=0;
   }
}



void hand_Release()   //手爪放松
{
  while(i==1)
  {
    for(hand_angle=max_hand_angle;hand_angle>min_hand_angle;hand_angle--)
   {
   servo_hand.write(hand_angle);  //设定舵机旋转角度的语句，可设定的角度范围是0°到180°。
   delay(30);   
   }
 //   hand_angle= myservo.read();
    Serial.print("手爪放松，此时角度为");
    Serial.println(hand_angle);
    i=0;
   }
}


void serial_control()   //由串口获取x_chassis,y_chassis值
{
  while(i==1)
  {
   //由串口获取x_chassis值
  if(Serial.available()>0){  
      delay(100);
    if(serial_control_factor==0){
      x_chassis = Serial.parseInt();  
      Serial.print("x_chassis:");        
      Serial.println(x_chassis); 
      serial_control_factor=1;
     }
  //由串口获取x_chassis值
   if(serial_control_factor==1){
      y_chassis = Serial.parseInt(); 
      Serial.print("y_chassis:");
      Serial.println(y_chassis); 
      serial_control_factor=0;
    }  
    // 清除缓存
    while(Serial.read() >= 0){}    
    Serial.println("输入成功！");
    i=0;
  }
  

   }
  
}