158 lines
2.9 KiB
C++
158 lines
2.9 KiB
C++
|
|
|
|
#include <Servo.h>
|
|
|
|
#define SERVO_L_MAX 80
|
|
#define SERVO_R_MAX 80
|
|
#define SERVO_C_MAX 110
|
|
|
|
#define SERVO_L_MIN 20
|
|
#define SERVO_R_MIN 20
|
|
#define SERVO_C_MIN 10
|
|
|
|
#define SERVO_L_CENTER 45
|
|
#define SERVO_R_CENTER 45
|
|
#define SERVO_C_CENTER 60
|
|
|
|
#define SERVO_STEP_DELAY 2
|
|
|
|
Servo servo_l;
|
|
Servo servo_r;
|
|
Servo servo_c;
|
|
|
|
|
|
|
|
|
|
void center_servos(){
|
|
servo_c.write(SERVO_C_CENTER);
|
|
delay(150);
|
|
servo_l.write(SERVO_L_CENTER);
|
|
delay(150);
|
|
servo_r.write(SERVO_R_CENTER);
|
|
delay(150);
|
|
|
|
}
|
|
|
|
void move_servo(Servo servo,int angle){
|
|
int current_angle=servo.read();
|
|
int i;
|
|
|
|
if (angle>current_angle)
|
|
for (i=current_angle;i<angle;i++){
|
|
servo.write(i);
|
|
delay(SERVO_STEP_DELAY);
|
|
}
|
|
else
|
|
for (i=current_angle;i>angle;i--){
|
|
servo.write(i);
|
|
delay(SERVO_STEP_DELAY);
|
|
}
|
|
|
|
}
|
|
|
|
void step_forward(){
|
|
move_servo(servo_c,SERVO_C_MIN);
|
|
move_servo(servo_l,SERVO_L_MAX);
|
|
move_servo(servo_c,SERVO_C_MAX);
|
|
move_servo(servo_l,SERVO_L_MIN);
|
|
move_servo(servo_r,SERVO_R_MIN);
|
|
move_servo(servo_c,SERVO_C_MIN);
|
|
move_servo(servo_r,SERVO_R_MAX);
|
|
move_servo(servo_c,SERVO_C_CENTER);
|
|
}
|
|
|
|
|
|
void run(int left,int right){
|
|
|
|
move_servo(servo_c,SERVO_C_MIN);
|
|
move_servo(servo_l,SERVO_L_MIN+left*10);
|
|
move_servo(servo_c,SERVO_C_MAX);
|
|
move_servo(servo_l,SERVO_L_MAX-left*10);
|
|
move_servo(servo_r,SERVO_R_MAX-right*10);
|
|
move_servo(servo_c,SERVO_C_MIN);
|
|
move_servo(servo_r,SERVO_R_MIN+right*10);
|
|
//move_servo(servo_c,SERVO_C_CENTER);
|
|
|
|
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
Serial1.begin(115200);
|
|
Serial.begin(115200);
|
|
delay(3000);
|
|
servo_l.attach(5);
|
|
servo_r.attach(10);
|
|
servo_c.attach(9);
|
|
//delay(100);
|
|
center_servos();
|
|
|
|
//delay(5000);
|
|
//move_servo(servo_c,SERVO_C_MAX);
|
|
//move_servo(servo_c,SERVO_C_MIN);
|
|
//servo_c.write(10);
|
|
//step_forward();
|
|
|
|
// run(6,6);
|
|
// delay(1000);
|
|
//
|
|
// run(0,0);
|
|
// delay(1000);
|
|
//
|
|
// run(3,3);
|
|
// delay(1000);
|
|
//
|
|
// run(6,0);
|
|
// delay(1000);
|
|
}
|
|
|
|
byte byteRead;
|
|
|
|
void loop()
|
|
{
|
|
//center_servos();
|
|
//servo_c.write(60);
|
|
//delay(50);
|
|
//servo_l.write(45);
|
|
//delay(50);
|
|
//move_servo(servo_c,SERVO_C_MAX);
|
|
//move_servo(servo_c,SERVO_C_MIN);
|
|
//step_forward();
|
|
|
|
while (Serial.available()||Serial1.available()) {
|
|
/* read the most recent byte */
|
|
|
|
if (Serial.available())
|
|
byteRead = Serial.read();
|
|
else
|
|
byteRead = Serial1.read();
|
|
//Serial.write(byteRead);
|
|
/*ECHO the value that was read, back to the serial port. */
|
|
if (byteRead=='w')
|
|
run(6,6);
|
|
else if(byteRead=='s')
|
|
run(0,0);
|
|
else if(byteRead=='a')
|
|
run(0,6);
|
|
else if(byteRead=='d')
|
|
run(6,0);
|
|
else if(byteRead=='x')
|
|
center_servos();
|
|
else if(byteRead=='q')
|
|
run(3,6);
|
|
else if(byteRead=='e')
|
|
run(6,3);
|
|
|
|
Serial.write(byteRead);
|
|
Serial1.write(byteRead);
|
|
|
|
if (!Serial.available())
|
|
Serial.println("rdy");
|
|
|
|
|
|
if (!Serial1.available())
|
|
Serial1.println("rdy");
|
|
}
|
|
center_servos();
|
|
}
|