Posted on  Updated on 

校巡线车校赛决赛代码

校赛第二名，净挣1000，当然不妨碍代码是垃圾代码，没啥必要发github,发发博客差不多得了。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
#include <Servo.h> 
#include <MsTimer2.h>

Servo myservo;  
                          
int pos = 70;   //舵机中值，对于我的舵机左->右 40-100
int lift = 20;
int right = 120;

// 舵机PWM引脚9，左电机PWM引脚5,右电机PWM引脚6；
int servo = 9;  int motor1=5;  int motor2=6;
int motor1_speed =60; int motor2_speed =60;

// 设置对管状态
int white = 0;
int black = 1;
int err,last_err; //pid 参数定义
int output;
float kp=10,ki=0,kd=0;
float P,I,D;

// 红外对管左->右引脚为2,3,4,7,8,12,13
int infrared[7]={2,3,4,7,8,12,13};

//实例特判
int zhijiao = 0;
int hu = 0;
int hu2 = 0;
int hu2_flag =0;
int final =0;


void flash() {
  final =1;
}


void setup() 
{ 
  myservo.attach(9);  // 该舵机由arduino第九脚控制
  myservo.write(pos);

  //电机pwm引脚初始化，模式设置为输出模式
  pinMode(motor1,OUTPUT);  
  pinMode(motor2,OUTPUT);
  pinMode(10,OUTPUT);

  Serial.begin(9600);//串口初始化
  //红外对管引脚模式设置为输入上拉模式
  pinMode(infrared[0],INPUT_PULLUP);
  pinMode(infrared[1],INPUT_PULLUP);
  pinMode(infrared[2],INPUT_PULLUP);
  pinMode(infrared[3],INPUT_PULLUP);
  pinMode(infrared[4],INPUT_PULLUP);
  pinMode(infrared[5],INPUT_PULLUP);
  pinMode(infrared[6],INPUT_PULLUP);

  //初始速度
  analogWrite(motor1,motor1_speed);
  analogWrite(motor2,motor2_speed);
}
void loop() 
{ 
  if(zhijiao == 0){
    zhijiao = 1;
    analogWrite(motor2,80);
    delay(1800);
    myservo.write(40);
    analogWrite(motor2,100);  
    delay(1000); 
  }
  if((hu == 0 && zhijiao == 1 && digitalRead(infrared[0])==black  && digitalRead(infrared[3])==black) || (hu == 0 && zhijiao == 1 && digitalRead(infrared[1])==black  && digitalRead(infrared[3])==black)){
    hu = 1;
    analogWrite(5,0);  
    analogWrite(6,0);
    analogWrite(11,100);
    myservo.write(70);
    delay(1800); 

    myservo.write(30);
    analogWrite(11,0);
    analogWrite(5,100);  
    analogWrite(6,100);
    delay(1500);

    
  }

  if(final==1 && digitalRead(infrared[1])==black && digitalRead(infrared[2])==black && digitalRead(infrared[3])==black){
  analogWrite(5,0);  
  analogWrite(6,0);
  delay(5500);     
  }

  if(hu==1  && hu2==0 && digitalRead(infrared[1])==black && digitalRead(infrared[2])==black && digitalRead(infrared[3])==black){
    hu2 = 1;
    analogWrite(5,0);  
    analogWrite(6,0);
    analogWrite(11,140);
    myservo.write(90);
    delay(2000); 
    myservo.write(30);
    analogWrite(11,0);
    analogWrite(5,120);  
    analogWrite(6,120);
    delay(1000);
    motor1_speed =70;motor2_speed =70;    
    MsTimer2::set(25000, flash); // 5000ms period
    MsTimer2::start();
  }
  sensor();
  int control_turn =pos + pid();
  myservo.write(control_turn);
  if(err == 4){
    analogWrite(motor1,motor1_speed+60);
  }
  else if(err == -4){
    analogWrite(motor2,motor2_speed+60);
  }
  else if(err == 2){
    analogWrite(motor1,motor1_speed+20);
    if(hu==1  && hu2==0){
      analogWrite(motor1,motor1_speed+40);
    }
  }
  else if(err == -2){
    analogWrite(motor2,motor2_speed+20);
    if(hu==1  && hu2==0){
      analogWrite(motor2,motor2_speed+40);
    }
  }
  else{
  analogWrite(motor1,motor1_speed);
  analogWrite(motor2,motor2_speed);    
  }
}

int pid(){
  P = err;
  I += err;
  D = err - last_err;
  output = kp*P + ki*I + kd*D;
  last_err = err;
  return output;
}

void sensor(){
  int a_1 = digitalRead(infrared[0]);
  int a_2 = digitalRead(infrared[1]);
  int a_3 = digitalRead(infrared[2]);
  int a_4 = digitalRead(infrared[3]);
  int a_5 = digitalRead(infrared[4]);
  int a_6 = digitalRead(infrared[5]);
  int a_7 = digitalRead(infrared[6]);
  if(a_1==black && a_2==white && a_3==white && a_4==white && a_5==white && a_6==white && a_7==white){
      err = -4;
  }
  else if(a_1==white && a_2==black && a_3==white && a_4==white && a_5==white && a_6==white && a_7==white){
      err = -2;
  }
  else if(a_1==white && a_2==white && a_3==black && a_4==white && a_5==white && a_6==white && a_7==white){
      err = -1;
  }
  else if(a_1==white && a_2==white && a_3==white && a_4==black && a_5==white && a_6==white && a_7==white){
      err = 0;
  } 
  else if(a_1==white && a_2==white && a_3==white && a_4==white && a_5==black && a_6==white && a_7==white){
      err = 1;
  }
  else if(a_1==white && a_2==white && a_3==white && a_4==white && a_5==white && a_6==black && a_7==white){
      err = 2;
  }  
  else if(a_1==white && a_2==white && a_3==white && a_4==white && a_5==white && a_6==white && a_7==black){
      err = 4;
  }
  Serial.println(err);  //测试代码
}
License

本文采用 署名-非商业性使用-相同方式共享 4.0 国际 许可协议，转载请注明出处。