01点40分2018年12月18日

其他 2019-01-08 17:11:03 阅读次数: 0

car循迹


//           电机设置             //
#define leftA_PIN 7
#define leftB_PIN 8
#define righA_PIN 3
#define righB_PIN 2

//PWM
int ENA1=6;  //电机A使能PWM输出，需要接在Arduino上有~标识的IO口，才有PWM
int ENB1=5;  //电机A使能PWM输出
//             循迹模块设置               //
#define leftA_track_PIN 13
#define leftB_track_PIN 12
#define middle_track_PIN 11
#define righA_track_PIN 10
#define righB_track_PIN 9

float Kp = 10, Ki = 0.5, Kd = 0;                    //pid弯道参数参数 
int error = 0, P = 0, I = 0, D = 0, PID_value = 0;//pid直道参数 
float decide = 0;                                   //元素判断
float previous_error = 0, previous_I = 0;           //误差值 
int sensor[5] = {0, 0, 0, 0, 0};                    //
int sensor_record[3] = {1,1,1};
int crosslinenumber[3] = {0,0,0};//跨黑线数目
static int initial_motor_speed = 60;                //初始速度 

/*电机引脚初始化*/
void motor_pinint( )
{
  pinMode (leftA_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (leftB_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (righA_PIN, OUTPUT); //设置引脚为输出引脚
  pinMode (righB_PIN, OUTPUT); //设置引脚为输出引脚
  analogWrite(ENA1,255);//左轮
  analogWrite(ENB1,250);//右轮
  }
  
/*循迹模块引脚初始化*/
void track_pinint()
{ 
  pinMode (leftA_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (leftB_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (middle_track_PIN, INPUT);//设置引脚为输入引脚
  pinMode (righA_track_PIN, INPUT); //设置引脚为输入引脚
  pinMode (righB_track_PIN, INPUT); //设置引脚为输入引脚
  
}

void forward(){
  analogWrite(ENA1,255);//左轮
  analogWrite(ENB1,250);//右轮
  digitalWrite(leftA_PIN,HIGH); //给高电平
  digitalWrite(leftB_PIN,LOW);  //给低电平
  digitalWrite(righA_PIN,HIGH); //给高电平
  digitalWrite(righB_PIN,LOW);  //给低电平
}
void turnleft(){

  digitalWrite(leftA_PIN,LOW); //给高电平
  digitalWrite(leftB_PIN,HIGH);  //给低电平
  digitalWrite(righA_PIN,HIGH); //给高电平
  digitalWrite(righB_PIN,LOW);  //给低电平
}
void turnright(){

  digitalWrite(leftA_PIN,HIGH); //给高电平
  digitalWrite(leftB_PIN,LOW);  //给低电平
  digitalWrite(righA_PIN,LOW); //给高电平
  digitalWrite(righB_PIN,HIGH);  //给低电平
}
void carstop()
{
  analogWrite(leftA_PIN,LOW);      
  analogWrite(leftB_PIN,LOW);         //左轮静止不动
  analogWrite(righA_PIN,LOW);      
  analogWrite(righB_PIN,LOW);         //右轮静止不动
}
void backward(){
  analogWrite(ENA1,250);//左轮
  analogWrite(ENB1,255);//右轮
  digitalWrite(leftA_PIN,LOW); //给高电平
  digitalWrite(leftB_PIN,HIGH);  //给低电平
  digitalWrite(righA_PIN,LOW); //给高电平
  digitalWrite(righB_PIN,HIGH);  //给低电平
}
void setup() {
  Serial.begin(9600);
  track_pinint();
  motor_pinint();
}

void read_sensor_values()
{
  sensor[0] = digitalRead(leftA_track_PIN);//一开始是1, 遇到黑线变为0，就是当下降沿计数
  sensor[1] = digitalRead(leftB_track_PIN);
  sensor[2] = digitalRead(middle_track_PIN);
  sensor[3] = digitalRead(righA_track_PIN);
  sensor[4] = digitalRead(righB_track_PIN);
  
  if(sensor_record[0]&&sensor[0]==0){
    crosslinenumber[0] += 1;   
    //Serial.print("左边跨过黑线一次");
    //Serial.println(crosslinenumber[0]);
    }
  if(sensor_record[1]&&sensor[4]==0){
    crosslinenumber[1] += 1;   
    Serial.print("右边跨过黑线一次");
    Serial.println(crosslinenumber[1]);
    }   
  if(sensor_record[2]&&sensor[2]==0){
    crosslinenumber[2] += 1;   
   // Serial.print("右边跨过黑线一次");
    //Serial.println(crosslinenumber[2]);
    }  
  sensor_record[0]=sensor[0];
  sensor_record[1]=sensor[4];
  sensor_record[2]=sensor[2];
  if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0) && (sensor[3] == 1) && (sensor[4] == 1)) {
      error = 0;}// 00100直行11011
  else if((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 0) && (sensor[4] == 1)) {
      error = 1;}//01000右转10111
  else if((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 0)) {
      error = 2;}//10000大右转 01111  
  else if((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
      error = -1;}//00010左转11101
  else if((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1) && (sensor[3] == 1) && (sensor[4] == 1)) {
      error = -2;}//00001大左转11110
  else{
    error=0;}            
}

void motor_control()
 {
  switch(error){
    case 0:
          forward();
          break;
    case 1:
          turnright();
          break;
    case 2:
          turnright();
          break;
    case -1:
          turnleft();
          break;
    case -2:
          turnleft();
          break;
  }
  }
void  goalittle(){ 
        read_sensor_values();//读取循迹讯号
        motor_control();//电机控制
        delay(1000);
} 
 
void loop() {

   //while(true){backward();}
    //Serial.println ("小车运行成功");
    while(true){
      read_sensor_values();//读取循迹讯号
      motor_control();//电机控制
      if (crosslinenumber[1]>=6){
         forward();
        delay(500); 
        carstop();
        Serial.println ("小车停止运行");
        break;
        }
    }
    //识别二维码完成，倒车
    Serial.println ("识别二维码完成，倒车");
    delay(2000);
    crosslinenumber[1]=0;
    while(true){ 
      read_sensor_values();//读取循迹讯号                
      backward();
      if (crosslinenumber[1]>=3){    
        backward();
        delay(200);
        carstop();
        Serial.println ("小车停止运行");
        break;
        }
      }
   //抓取完毕，转向   
   Serial.println ("抓取完毕，转向 ");  
   delay(2000); 
   forward();
    delay(1200);  
    carstop(); 
    delay(2000);
    crosslinenumber[1]=0;
    crosslinenumber[0]=0;
    crosslinenumber[2]=0;
   
    while(true){ 
      read_sensor_values();//读取循迹讯号                     
      turnleft();
      
      if (crosslinenumber[0]>=1&& crosslinenumber[2]>=2){
           
        carstop();
        Serial.println ("小车停止运行");
        break;
        
        }
      }

    //转向完毕，开始直行，投放
    Serial.println ("转向完毕，开始直行，投放 "); 
    delay(2000); 
    crosslinenumber[1]=0;
    while(true){   
      read_sensor_values();//读取循迹讯号              
      motor_control();//电机控制
      if (crosslinenumber[1]>=4){
        carstop();
        Serial.println ("小车停止运行");
        break;
        }
      }
      
      while(true){
        Serial.println ("小车停止运行");
        carstop();
        delay(2000);
        }
} //loop

hand

/*************************************************** 
  这是16通道PWM和伺服驱动器的一个例子，驱动16个伺服电机
  ****************************************************/

#include <Wire.h>                    //16路舵机控制板头文件
#include <Adafruit_PWMServoDriver.h> //16路舵机控制板头文件
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();//以这种方式调用，它使用默认地址0x40。

#define SERVOMIN  150   //这是“最小”脉冲长度计数（在4096中）
#define SERVOMAX  300//600   //这是“最大”脉冲长度计数（在4096中）
//dregee=10/3*real_degree+150
/******************************
 * 设定值：
 * 
 ******************************/
int angle[7]={40,55,60,60,60,60,0};//初始角度
int max_angle[7]={65,0,60,60,60,60,180};
int channel[7]={0,2,4,6,8,10,12};
int hand_speed=5;
void setup() {
   Serial.begin(9600);
   Serial.println("16 channel Servo test!");
   pwm.begin();
   pwm.setPWMFreq(60);  //设置频率60Hz  可用50Hz 40-1000//测试对电机的速度没有影响
   for(int i=0;i<7;i++){
    angle[i]=int(10/3*angle[i]+150);//change angle
    max_angle[i]=int(10/3*max_angle[i]+150);
    }
}

//角度换算
int deg(float real_degree){
  return(int(10/3*real_degree+150));
  }
int antideg(float degree){
  return(int((degree-150)*3/10));
  }
  
void pwm_text(){     //16路舵机控制板测试函数，效果是来回转动，16路同时
 //每次驱动一个伺服驱动器
   for (uint16_t pulse = SERVOMIN; pulse < SERVOMAX; pulse++) 
   {
    for(uint8_t channel=0;channel<2;channel++)
      {
       pwm.setPWM(channel, 0, pulse);    //pulse翻译为脉冲  
       delay(5);
      }     
    }
  delay(500);

   for (uint16_t pulse = SERVOMAX; pulse > SERVOMIN; pulse--) 
   {
    for(uint8_t channel=0;channel<2;channel++)
     {
      pwm.setPWM(channel, 0, pulse);    //pulse翻译为脉冲  
      delay(5);
      }   
    } 
     delay(500);

}

//手指张合
void servo_finger(){
  
   for(int i=angle[0];i<max_angle[0];i++){
      pwm.setPWM(0, 0, i);
      int j=angle[1]-(i-max_angle[0]);//电机对称角度换算
      pwm.setPWM(2, 0, j);
      delay(hand_speed);
      Serial.print("i=,j=");
      Serial.print(i);
      Serial.print(",");
      Serial.println(j);
   }
   for(int i=max_angle[0];i>angle[0];i--){
      pwm.setPWM(0, 0, i);
      int j=angle[1]-(i-max_angle[0]);
      pwm.setPWM(2, 0, j);
      delay(hand_speed);
      Serial.print("i=,j=");
      Serial.print(i);
      Serial.print(",");
      Serial.println(j);
   }

}

void servo_6(){
  for (int i=angle[6];i<max_angle[6];i++){
    pwm.setPWM(channel[6], 0, i);    //pulse翻译为脉冲  
    delay(hand_speed);
    }
  for (int i=max_angle[6];i>angle[6];i--){
    pwm.setPWM(channel[6], 0, i);    //pulse翻译为脉冲  
    delay(hand_speed);
    }  
}

void hand_reset(){
    pwm.setPWM(0, 0, deg(65));    //pulse翻译为脉冲  
    pwm.setPWM(2, 0, deg(55));    //pulse翻译为脉冲
    pwm.setPWM(4, 0, deg(20));    //pulse翻译为脉冲
    pwm.setPWM(6, 0, deg(60));    //pulse翻译为脉冲
    pwm.setPWM(8, 0, deg(20));    //pulse翻译为脉冲
    pwm.setPWM(10, 0, deg(70));    //pulse翻译为脉冲
    pwm.setPWM(12, 0, deg(0));    //pulse翻译为脉冲
    
}


int finger_degree[]={deg(65),deg(65)-15,deg(65)-24,deg(65)-36,deg(65)-40};
int turn[]={deg(0),deg(0)+5,deg(0)+12,deg(0)+25,deg(0)+40};

void handmove(int door, int reset_degree, int goal_degree){
  reset_degree=deg(reset_degree);
  goal_degree=deg(goal_degree);
  while(true){
  if(reset_degree<goal_degree){
    reset_degree=reset_degree+1;    
  }
  else if(reset_degree>goal_degree){
    reset_degree=reset_degree-1;   
  }
  else{break;}
  Serial.print("reset_degree=");
  Serial.println(reset_degree);
  pwm.setPWM(door, 0, reset_degree);    //pulse翻译为脉冲
  delay(10);
  }
}

/********************主函数********************/

void loop(){
 // hand_reset();
    handmove(12,130,0);
    while(true){}
  
}


void text2() {
   //pwm_text();     //16路舵机控制板测试函数，效果是来回转动，16路同时
  //servo_finger();
  hand_reset();
  delay(1000);
  //servo_6();
 while(true){
    for (int i=0;i<5;i++){
    pwm.setPWM(0, 0, finger_degree[i]);    //pulse翻译为脉冲        
    pwm.setPWM(channel[6], 0, turn[i]);    //pulse翻译为脉冲
    delay(200);     
    }
    
    break;
 }
 while(true){
  
 }
}



/*************************************************** 
  这是16通道PWM和伺服驱动器的一个例子，驱动16个伺服电机
  ****************************************************/

#include <Wire.h>                    //16路舵机控制板头文件
#include <Adafruit_PWMServoDriver.h> //16路舵机控制板头文件
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();//以这种方式调用，它使用默认地址0x40。

#define SERVOMIN  150   //这是“最小”脉冲长度计数（在4096中）
#define SERVOMAX  300//600   //这是“最大”脉冲长度计数（在4096中）
//dregee=10/3*real_degree+150
/******************************
 * 设定值：
 * 
 ******************************/
int angle[7]={40,55,0,0,0,0,0};//初始角度
int max_angle[7]={65,0,160,160,160,160,180};
int angle_error[7]={0,0,0,0,0,0,0};
int max_error=0;
int max_error_chanel=0;
int channel[7]={0,2,4,6,8,10,12};
int hand_speed=5;
void setup() {
   Serial.begin(9600);
   Serial.println("16 channel Servo test!");
   pwm.begin();
   pwm.setPWMFreq(60);  //设置频率60Hz  可用50Hz 40-1000//测试对电机的速度没有影响
   for(int i=0;i<7;i++){
    angle[i]=int(2.5*angle[i]+150);//change angle
    max_angle[i]=float(2.5*max_angle[i]+150);
    }
}

//角度换算
int deg(float real_degree){
  return(int(2.5*real_degree+150));
  }
int antideg(float degree){
  return(int((degree-150)*3/10));
  }
  
void pwm_text(){     //16路舵机控制板测试函数，效果是来回转动，16路同时
 //每次驱动一个伺服驱动器
   for (uint16_t pulse = SERVOMIN; pulse < SERVOMAX; pulse++) 
   {
    for(uint8_t channel=0;channel<2;channel++)
      {
       pwm.setPWM(channel, 0, pulse);    //pulse翻译为脉冲  
       delay(5);
      }     
    }
  delay(500);

   for (uint16_t pulse = SERVOMAX; pulse > SERVOMIN; pulse--) 
   {
    for(uint8_t channel=0;channel<2;channel++)
     {
      pwm.setPWM(channel, 0, pulse);    //pulse翻译为脉冲  
      delay(5);
      }   
    } 
     delay(500);

}

//手指张合
void servo_finger(){
  
   for(int i=angle[0];i<max_angle[0];i++){
      pwm.setPWM(0, 0, i);
      int j=angle[1]-(i-max_angle[0]);//电机对称角度换算
      pwm.setPWM(2, 0, j);
      delay(hand_speed);
      Serial.print("i=,j=");
      Serial.print(i);
      Serial.print(",");
      Serial.println(j);
   }
   for(int i=max_angle[0];i>angle[0];i--){
      pwm.setPWM(0, 0, i);
      int j=angle[1]-(i-max_angle[0]);
      pwm.setPWM(2, 0, j);
      delay(hand_speed);
      Serial.print("i=,j=");
      Serial.print(i);
      Serial.print(",");
      Serial.println(j);
   }

}

void servo_6(){
  for (int i=angle[6];i<max_angle[6];i++){
    pwm.setPWM(channel[6], 0, i);    //pulse翻译为脉冲  
    delay(hand_speed);
    }
  for (int i=max_angle[6];i>angle[6];i--){
    pwm.setPWM(channel[6], 0, i);    //pulse翻译为脉冲  
    delay(hand_speed);
    }  
}

void hand_reset(){
    pwm.setPWM(0, 0, deg(65));    //pulse翻译为脉冲  
    pwm.setPWM(2, 0, deg(55));    //pulse翻译为脉冲
    pwm.setPWM(4, 0, deg(20));    //pulse翻译为脉冲
    pwm.setPWM(6, 0, deg(60));    //pulse翻译为脉冲
    pwm.setPWM(8, 0, deg(20));    //pulse翻译为脉冲
    pwm.setPWM(10, 0, deg(70));    //pulse翻译为脉冲
    pwm.setPWM(12, 0, deg(0));    //pulse翻译为脉冲
    
}


int finger_degree[]={deg(65),deg(65)-15,deg(65)-24,deg(65)-36,deg(65)-40};
int turn[]={deg(0),deg(0)+5,deg(0)+12,deg(0)+25,deg(0)+40};

void handmove(int door, int reset_degree, int goal_degree){
  reset_degree=deg(reset_degree);
  goal_degree=deg(goal_degree);
  while(true){
  if(reset_degree<goal_degree){
    reset_degree=reset_degree+1;    
  }
  else if(reset_degree>goal_degree){
    reset_degree=reset_degree-1;   
  }
  else{break;}
  Serial.print("reset_degree=");
  Serial.println(reset_degree);
  pwm.setPWM(door, 0, reset_degree);    //pulse翻译为脉冲
  delay(10);
  }
}

void findmax_error(){
      for(int i=0;i<7;i++){
      angle_error[i]=abs(angle[i]-max_angle[i]);
      //Serial.print("angle_error[i]="); 
      //Serial.println(angle_error[i]);
      }
      for(int i=1;i<7;i++){
       max_error=max(angle_error[i],max_error);
       if(max_error==angle_error[i]){
        max_error_chanel=i;
       }
      }
      Serial.print("max_error="); 
      Serial.println(max_error);
      Serial.print("max_error_chanel="); 
      Serial.println(max_error_chanel);    
      

//loop      
}
 
 
/********************主函数********************/
void loop(){
      findmax_error();    
      int cutdegree[7][450];
      int firstangle=angle[6];     
      for(int i=0;i<450; i++){
        firstangle=firstangle+1;
        cutdegree[6][i]= firstangle;
        Serial.print("cutdegree[max_error_chanel][i]="); 
        Serial.println(cutdegree[6][i]);
        pwm.setPWM(channel[max_error_chanel], 0, cutdegree[6][i]);
        delay(3);
      }
      while(1){}

//loop      
}




    
  



void text2() {
   //pwm_text();     //16路舵机控制板测试函数，效果是来回转动，16路同时
  //servo_finger();
  hand_reset();
  delay(1000);
  //servo_6();
 while(true){
    for (int i=0;i<5;i++){
    pwm.setPWM(0, 0, finger_degree[i]);    //pulse翻译为脉冲        
    pwm.setPWM(channel[6], 0, turn[i]);    //pulse翻译为脉冲
    delay(200);     
    }
    
    break;
 }
 while(true){
  
 }
}
/********************************************/

猜你喜欢

转载自blog.csdn.net/qq_42807924/article/details/85059137
今日推荐
开源日报 | Chrome内置Gemini的意义不在于Gemini；中国AI追随之路的五大误区；ECharts创始人“下海”养鱼；谷歌I/O开发者大会什么都有，只是没有惊喜
微软回应中国区AI团队“打包赴美”传闻
基于大语言模型的开源知识库问答系统 MaxKB GitHub Star 数量突破 5,000 个！
美国拟限制 AI 大模型出口中国和俄罗斯
苹果将与 OpenAI 达成协议，将 ChatGPT 应用于 iPhone
openKylin 社区生态委员会第六次会议圆满召开
阿里云正式发布通义千问 2.5
Python 3.13 发布首个 Beta：实验性自由线程模式和 JIT、改进交互式解释器
Stack Overflow 拿我的代码去训练 AI 大模型，还封了我的账号
Pop!_OS 的 COSMIC 桌面完成 App Store 上架工作
《2024 年一季度互联网投融资运行情况》研究报告
报告：Django 仍然是 74% 开发者的首选
周排行
返回指定时间格式
fopen函数中的mode参数
Java 单例模式探讨
Flex remoteobject工作原理探讨
寻找mplayer的便捷安装方法
30天了解30种技术系列---(26)MySQL自动化运维工具Inception
关于Jboss/Tomcat/Jetty的JNDI定义123
程序减肥，strip，eu-strip 及其符号表
AsyncTask、View.post(Runnable)、ViewTreeObserver三种方式总结frame animation自动启动
Json和Bean的互相转换
每日归档
更多
2024-05-15(24)
2024-05-14(0)
2024-05-13(18)
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)
2024-05-09(35)
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022