#include< PS2X_lib.h> //for v1.6
#define PS2_DAT A0 //14
#define PS2_CMD A1 //15
#define PS2_SEL A2 //16
#define PS2_CLK A3 //17
#define pressures true
#define rumble true
PS2X ps2x;
int error = 0;
byte type = 0;
byte vibrate = 0;
#define left_turn 0x06//按键左旋转
#define right_turn 0x07//按键右旋转
struct _pid{
float SetSpeed; //定义设定值
float ActualSpeed; //定义实际值
float err; //定义偏差值
float err_last; //定义上一个偏差值
float Kp,Ki,Kd; //定义比例、积分、微分系数
float voltage; //定义电压值(控制执行器的变量)
float integral; //定义积分值
}pid;
void PID_init(){ //PID初始化,调整参数
printf("PID_init begin \n");
pid.SetSpeed=0.0;
pid.ActualSpeed=0.0;
pid.err=0.0;
pid.err_last=0.0;
pid.voltage=0.0;
pid.integral=0.0;
pid.Kp=0.2;
pid.Ki=0.015;
pid.Kd=0.2;
printf("PID_init end \n");
}
float PID_realize(float speed){
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
pid.integral+=pid.err;
pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err;
pid.ActualSpeed=pid.voltage*1.0;
return pid.ActualSpeed;
}
enum {
enSTOP = 1,
enRUN,
enBACK,
enLEFT,
enRIGHT,
enUPLEFT,
enUPRIGHT,
enDOWNLEFT,
enDOWNRIGHT
} enCarState;
#define level1 0x08//速度控制标志位1
#define level2 0x09//速度控制标志位2
#define level3 0x0A//速度控制标志位3
#define level4 0x0B//速度控制标志位4
#define level5 0x0C//速度控制标志位5
#define level6 0x0D//速度控制标志位6
#define level7 0x0E//速度控制标志位7
#define level8 0x0F//速度控制标志位8
int Left_motor_back = 9; //左电机后退(IN1)
int Left_motor_go = 5; //左电机前进(IN2)
int Right_motor_go = 6; // 右电机前进(IN3)
int Right_motor_back = 10; // 右电机后退(IN4)
int buzzer = 8;//设置控制蜂鸣器的数字IO脚
int control = 150;//PWM控制量
int g_carstate = enSTOP; // 1前2后3左4右0停止
int g_servostate = 0; //1左摇 2 右摇
/*超声波*/
int Echo = A5; // Echo回声脚(P2.0)
int Trig = A4; // Trig 触发脚(P2.1)
int Distance = 0;
/*舵机*/
int servopin = 2; //设置舵机驱动脚到数字口2
/*点灯*/
int Led = 13; //
/*灭火*/
int Fire = 12; //
int speakerPin = 3; //11
void PS2_Ctrol(void);
void setup()
{
//初始化电机驱动IO为输出方式
pinMode(Left_motor_go, OUTPUT); // PIN 5 (PWM)
pinMode(Left_motor_back, OUTPUT); // PIN 9 (PWM)
pinMode(Right_motor_go, OUTPUT); // PIN 6 (PWM)
pinMode(Right_motor_back, OUTPUT); // PIN 10 (PWM)
pinMode(buzzer, OUTPUT); //设置数字IO脚模式,OUTPUT为输出
pinMode(Echo, INPUT); // 定义超声波输入脚
pinMode(Trig, OUTPUT); // 定义超声波输出脚
pinMode(servopin, OUTPUT); //设定舵机接口为输出接口
pinMode(Led, OUTPUT); // 定义点灯输出脚
pinMode(Fire, OUTPUT); // 定义灭火输出
pinMode(speakerPin, OUTPUT); //定义唱歌引脚
//digitalWrite(buzzer,HIGH); //不发声
// digitalWrite(Led,HIGH);
// digitalWrite(Fire,HIGH);
Serial.begin(9600); //波特率9600 (蓝牙通讯设定波特率)
/*PS2初始化*/
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
type = ps2x.readType();
switch (type) {
case 0:
Serial.print("Unknown Controller type found ");
break;
case 1:
Serial.print("DualShock Controller found "); //这种
break;
case 2:
Serial.print("GuitarHero Controller found ");
break;
case 3:
Serial.print("Wireless Sony DualShock Controller found ");
break;
}
}
void Distance_test() // 量出前方距离
{
digitalWrite(Trig, LOW); // 给触发脚低电平2μs
delayMicroseconds(2);
digitalWrite(Trig, HIGH); // 给触发脚高电平10μs,这里至少是10μs
delayMicroseconds(10);
digitalWrite(Trig, LOW); // 持续给触发脚低电
float Fdistance = pulseIn(Echo, HIGH); // 读取高电平时间(单位:微秒)
Fdistance = Fdistance / 58; //为什么除以58等于厘米, Y米=(X秒*344)/2
// X秒=( 2*Y米)/344 ==》X秒=0.0058*Y米 ==》厘米=微秒/58
//Serial.print("Distance:"); //输出距离(单位:厘米)
//Serial.println(Fdistance); //显示距离
Distance = Fdistance;
}
void run() // 前进
{
int speed=PID_realize(200.0);
analogWrite(Right_motor_back, speed);
analogWrite(Left_motor_back, speed);
analogWrite(Right_motor_go, control); //PWM比例0~255调速,左右轮差异略增减
analogWrite(Left_motor_go, control - 25); //PWM比例0~255调速,左右轮差异略增减
//delay(time * 100); //执行时间,可以调整
}
void brake() //刹车,停车
{
int speed=PID_realize(0.0);
analogWrite(Right_motor_back, speed);
analogWrite(Left_motor_back, speed);
//delay(time * 100);//执行时间,可以调整
}
void left() //左转(左轮不动,右轮前进)
{
int speed=PID_realize(200.0);
analogWrite(Right_motor_back, speed);
analogWrite(Left_motor_back, 0);
}
void right() //右转(右轮不动,左轮前进)
{
int speed=PID_realize(200.0);
analogWrite(Right_motor_back, speed);
analogWrite(Left_motor_back, speed);
}
void upright() //右上(右轮不动,左轮前进)
{
digitalWrite(Right_motor_back, LOW);
digitalWrite(Left_motor_back, LOW);
digitalWrite(Right_motor_go, HIGH); // 右电机前进
digitalWrite(Left_motor_go, HIGH); // 左电机前进
analogWrite(Right_motor_go, 120); //PWM比例0~255调速,左右轮差异略增减 右电机减速
analogWrite(Left_motor_go, 180); //PWM比例0~255调速,左右轮差异略增减
}
void front_detection()
{
//此处循环次数减少,为了增加小车遇到障碍物的反应速度
for (int i = 0; i< = 5; i++) //产生PWM个数,等效延时以保证能转到响应角度
{
// servopulse(servopin, 90); //模拟产生PWM
}
}
void left_detection()
{
for (int i = 0; i< = 15; i++) //产生PWM个数,等效延时以保证能转到响应角度
{
// servopulse(servopin, 175); //模拟产生PWM
}
}
void right_detection()
{
for (int i = 0; i< = 15; i++) //产生PWM个数,等效延时以保证能转到响应角度
{
// servopulse(servopin, 5); //模拟产生PWM
}
}
void loop()
{
int temp = 0;
PS2_Ctrol();
switch (g_carstate)
{
case enRUN: run(); break;
case enLEFT: left(); break;
case enRIGHT: right(); break;
//case enBACK: back(); break;
default: break;
}
switch (g_servostate)
{
case 0: if (temp != 0) {
temp = 0;
front_detection();
} break;
case 1: if (temp != 1) {
temp = 1;
left_detection();
} break;
case 2: if (temp != 2) {
temp = 2;
right_detection();
} break;
default: break;
}
delay(50);
}
void PS2_Ctrol(void)
{
int X1, Y1, X2, Y2;
if (error == 1) //skip loop if no controller found
return;
if (type != 1) //skip loop if no controller found
return;
//DualShock Controller
ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
if (ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed
Serial.println("Start is being held");
if (ps2x.Button(PSB_SELECT))
Serial.println("Select is being held");
if (ps2x.Button(PSB_PAD_UP)) { //will be TRUE as long as button is pressed
Serial.print("Up held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
g_carstate = enRUN;
}
else if (ps2x.Button(PSB_PAD_RIGHT)) {
Serial.print("Right held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
g_carstate = enRIGHT;
}
else if (ps2x.Button(PSB_PAD_LEFT)) {
Serial.print("LEFT held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
g_carstate = enLEFT;
}
else if (ps2x.Button(PSB_PAD_DOWN)) {
Serial.print("DOWN held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
g_carstate = enBACK;
}
else
{
g_carstate = enSTOP;
}
vibrate = ps2x.Analog(PSAB_CROSS); //this will set the large motor vibrate speed based on how hard you press the blue (X) button
if (ps2x.NewButtonState())
{ //will be TRUE if any button changes state (on to off, or off to on)
if (ps2x.Button(PSB_L3))//停止
{
g_carstate = enSTOP;
Serial.println("L3 pressed");
}
if (ps2x.Button(PSB_R3)) //复位
{
front_detection();
Serial.println("R3 pressed");
}
if (ps2x.Button(PSB_L2)) //加速
{
Serial.println("L2 pressed");
control += 50;
if (control > 255)
{
control = 255;
}
}
if (ps2x.Button(PSB_R2))
{
Serial.println("R2 pressed");
control -= 50;
if (control< 50)
{
control = 100;
}
}
if (ps2x.Button(PSB_TRIANGLE)) //点灯
{
Serial.println("Triangle pressed");
digitalWrite(Led, !digitalRead(Led)); //反转电平
}
}
if (ps2x.ButtonPressed(PSB_CIRCLE))//灭火 //will be TRUE if button was JUST pressed
{
Serial.println("Circle just pressed");
digitalWrite(Fire, !digitalRead(Fire)); //反转电平
}
if (ps2x.NewButtonState(PSB_CROSS))//鸣笛 //will be TRUE if button was JUST pressed OR released
{
Serial.println("X just changed");
// whistle();
}
if (ps2x.ButtonReleased(PSB_SQUARE))//唱歌 //will be TRUE if button was JUST released
{
Serial.println("Square just released");
//PlayTest();
}
if (ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1))
{ //print stick values if either is TRUE
Serial.print("Stick Values:");
Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
Serial.print(",");
Serial.print(ps2x.Analog(PSS_LX), DEC);
Serial.print(",");
Serial.print(ps2x.Analog(PSS_RY), DEC);
Serial.print(",");
Serial.println(ps2x.Analog(PSS_RX), DEC);
Y1 = ps2x.Analog(PSS_LY);
X1 = ps2x.Analog(PSS_LX);
Y2 = ps2x.Analog(PSS_RY);
X2 = ps2x.Analog(PSS_RX);
/*左摇杆*/
if (Y1< 5&& X1 > 80&& X1< 180) //上
{
g_carstate = enRUN;
}
else if (Y1 > 230&& X1 > 80&& X1< 180) //下
{
g_carstate = enBACK;
}
else if (X1< 5&& Y1 > 80&& Y1< 180) //左
{
g_carstate = enLEFT;
}
else if (Y1 > 80&& Y1< 180&& X1 > 230)//右
{
g_carstate = enRIGHT;
}
else if (Y1< = 80&& X1< = 80) //左上
{
g_carstate = enUPLEFT;
}
else if (Y1< = 80&& X1 >= 180) //右上
{
g_carstate = enUPRIGHT;
}
else if (X1< = 80&& Y1 >= 180) // 左下
{
g_carstate = enDOWNLEFT;
}
else if (Y1 >= 180&& X1 >= 180) //右下
{
g_carstate = enDOWNRIGHT;
}
else//停
{
g_carstate = enSTOP;
}
/*右摇杆*/
if (X2< 5&& Y2 > 110&& Y2< 150) //左
{
g_servostate = 1;
}
else if (Y2 > 110&& Y2< 150&& X2 > 230)//右
{
g_servostate = 2;
}
else//归位
{
g_servostate = 0;
}
}
}
#define PS2_DAT A0 //14
#define PS2_CMD A1 //15
#define PS2_SEL A2 //16
#define PS2_CLK A3 //17
#define pressures true
#define rumble true
PS2X ps2x;
int error = 0;
byte type = 0;
byte vibrate = 0;
#define left_turn 0x06//按键左旋转
#define right_turn 0x07//按键右旋转
struct _pid{
float SetSpeed; //定义设定值
float ActualSpeed; //定义实际值
float err; //定义偏差值
float err_last; //定义上一个偏差值
float Kp,Ki,Kd; //定义比例、积分、微分系数
float voltage; //定义电压值(控制执行器的变量)
float integral; //定义积分值
}pid;
void PID_init(){ //PID初始化,调整参数
printf("PID_init begin \n");
pid.SetSpeed=0.0;
pid.ActualSpeed=0.0;
pid.err=0.0;
pid.err_last=0.0;
pid.voltage=0.0;
pid.integral=0.0;
pid.Kp=0.2;
pid.Ki=0.015;
pid.Kd=0.2;
printf("PID_init end \n");
}
float PID_realize(float speed){
pid.SetSpeed=speed;
pid.err=pid.SetSpeed-pid.ActualSpeed;
pid.integral+=pid.err;
pid.voltage=pid.Kp*pid.err+pid.Ki*pid.integral+pid.Kd*(pid.err-pid.err_last);
pid.err_last=pid.err;
pid.ActualSpeed=pid.voltage*1.0;
return pid.ActualSpeed;
}
enum {
enSTOP = 1,
enRUN,
enBACK,
enLEFT,
enRIGHT,
enUPLEFT,
enUPRIGHT,
enDOWNLEFT,
enDOWNRIGHT
} enCarState;
#define level1 0x08//速度控制标志位1
#define level2 0x09//速度控制标志位2
#define level3 0x0A//速度控制标志位3
#define level4 0x0B//速度控制标志位4
#define level5 0x0C//速度控制标志位5
#define level6 0x0D//速度控制标志位6
#define level7 0x0E//速度控制标志位7
#define level8 0x0F//速度控制标志位8
int Left_motor_back = 9; //左电机后退(IN1)
int Left_motor_go = 5; //左电机前进(IN2)
int Right_motor_go = 6; // 右电机前进(IN3)
int Right_motor_back = 10; // 右电机后退(IN4)
int buzzer = 8;//设置控制蜂鸣器的数字IO脚
int control = 150;//PWM控制量
int g_carstate = enSTOP; // 1前2后3左4右0停止
int g_servostate = 0; //1左摇 2 右摇
/*超声波*/
int Echo = A5; // Echo回声脚(P2.0)
int Trig = A4; // Trig 触发脚(P2.1)
int Distance = 0;
/*舵机*/
int servopin = 2; //设置舵机驱动脚到数字口2
/*点灯*/
int Led = 13; //
/*灭火*/
int Fire = 12; //
int speakerPin = 3; //11
void PS2_Ctrol(void);
void setup()
{
//初始化电机驱动IO为输出方式
pinMode(Left_motor_go, OUTPUT); // PIN 5 (PWM)
pinMode(Left_motor_back, OUTPUT); // PIN 9 (PWM)
pinMode(Right_motor_go, OUTPUT); // PIN 6 (PWM)
pinMode(Right_motor_back, OUTPUT); // PIN 10 (PWM)
pinMode(buzzer, OUTPUT); //设置数字IO脚模式,OUTPUT为输出
pinMode(Echo, INPUT); // 定义超声波输入脚
pinMode(Trig, OUTPUT); // 定义超声波输出脚
pinMode(servopin, OUTPUT); //设定舵机接口为输出接口
pinMode(Led, OUTPUT); // 定义点灯输出脚
pinMode(Fire, OUTPUT); // 定义灭火输出
pinMode(speakerPin, OUTPUT); //定义唱歌引脚
//digitalWrite(buzzer,HIGH); //不发声
// digitalWrite(Led,HIGH);
// digitalWrite(Fire,HIGH);
Serial.begin(9600); //波特率9600 (蓝牙通讯设定波特率)
/*PS2初始化*/
error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);
type = ps2x.readType();
switch (type) {
case 0:
Serial.print("Unknown Controller type found ");
break;
case 1:
Serial.print("DualShock Controller found "); //这种
break;
case 2:
Serial.print("GuitarHero Controller found ");
break;
case 3:
Serial.print("Wireless Sony DualShock Controller found ");
break;
}
}
void Distance_test() // 量出前方距离
{
digitalWrite(Trig, LOW); // 给触发脚低电平2μs
delayMicroseconds(2);
digitalWrite(Trig, HIGH); // 给触发脚高电平10μs,这里至少是10μs
delayMicroseconds(10);
digitalWrite(Trig, LOW); // 持续给触发脚低电
float Fdistance = pulseIn(Echo, HIGH); // 读取高电平时间(单位:微秒)
Fdistance = Fdistance / 58; //为什么除以58等于厘米, Y米=(X秒*344)/2
// X秒=( 2*Y米)/344 ==》X秒=0.0058*Y米 ==》厘米=微秒/58
//Serial.print("Distance:"); //输出距离(单位:厘米)
//Serial.println(Fdistance); //显示距离
Distance = Fdistance;
}
void run() // 前进
{
int speed=PID_realize(200.0);
analogWrite(Right_motor_back, speed);
analogWrite(Left_motor_back, speed);
analogWrite(Right_motor_go, control); //PWM比例0~255调速,左右轮差异略增减
analogWrite(Left_motor_go, control - 25); //PWM比例0~255调速,左右轮差异略增减
//delay(time * 100); //执行时间,可以调整
}
void brake() //刹车,停车
{
int speed=PID_realize(0.0);
analogWrite(Right_motor_back, speed);
analogWrite(Left_motor_back, speed);
//delay(time * 100);//执行时间,可以调整
}
void left() //左转(左轮不动,右轮前进)
{
int speed=PID_realize(200.0);
analogWrite(Right_motor_back, speed);
analogWrite(Left_motor_back, 0);
}
void right() //右转(右轮不动,左轮前进)
{
int speed=PID_realize(200.0);
analogWrite(Right_motor_back, speed);
analogWrite(Left_motor_back, speed);
}
void upright() //右上(右轮不动,左轮前进)
{
digitalWrite(Right_motor_back, LOW);
digitalWrite(Left_motor_back, LOW);
digitalWrite(Right_motor_go, HIGH); // 右电机前进
digitalWrite(Left_motor_go, HIGH); // 左电机前进
analogWrite(Right_motor_go, 120); //PWM比例0~255调速,左右轮差异略增减 右电机减速
analogWrite(Left_motor_go, 180); //PWM比例0~255调速,左右轮差异略增减
}
void front_detection()
{
//此处循环次数减少,为了增加小车遇到障碍物的反应速度
for (int i = 0; i< = 5; i++) //产生PWM个数,等效延时以保证能转到响应角度
{
// servopulse(servopin, 90); //模拟产生PWM
}
}
void left_detection()
{
for (int i = 0; i< = 15; i++) //产生PWM个数,等效延时以保证能转到响应角度
{
// servopulse(servopin, 175); //模拟产生PWM
}
}
void right_detection()
{
for (int i = 0; i< = 15; i++) //产生PWM个数,等效延时以保证能转到响应角度
{
// servopulse(servopin, 5); //模拟产生PWM
}
}
void loop()
{
int temp = 0;
PS2_Ctrol();
switch (g_carstate)
{
case enRUN: run(); break;
case enLEFT: left(); break;
case enRIGHT: right(); break;
//case enBACK: back(); break;
default: break;
}
switch (g_servostate)
{
case 0: if (temp != 0) {
temp = 0;
front_detection();
} break;
case 1: if (temp != 1) {
temp = 1;
left_detection();
} break;
case 2: if (temp != 2) {
temp = 2;
right_detection();
} break;
default: break;
}
delay(50);
}
void PS2_Ctrol(void)
{
int X1, Y1, X2, Y2;
if (error == 1) //skip loop if no controller found
return;
if (type != 1) //skip loop if no controller found
return;
//DualShock Controller
ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed
if (ps2x.Button(PSB_START)) //will be TRUE as long as button is pressed
Serial.println("Start is being held");
if (ps2x.Button(PSB_SELECT))
Serial.println("Select is being held");
if (ps2x.Button(PSB_PAD_UP)) { //will be TRUE as long as button is pressed
Serial.print("Up held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_UP), DEC);
g_carstate = enRUN;
}
else if (ps2x.Button(PSB_PAD_RIGHT)) {
Serial.print("Right held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_RIGHT), DEC);
g_carstate = enRIGHT;
}
else if (ps2x.Button(PSB_PAD_LEFT)) {
Serial.print("LEFT held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_LEFT), DEC);
g_carstate = enLEFT;
}
else if (ps2x.Button(PSB_PAD_DOWN)) {
Serial.print("DOWN held this hard: ");
Serial.println(ps2x.Analog(PSAB_PAD_DOWN), DEC);
g_carstate = enBACK;
}
else
{
g_carstate = enSTOP;
}
vibrate = ps2x.Analog(PSAB_CROSS); //this will set the large motor vibrate speed based on how hard you press the blue (X) button
if (ps2x.NewButtonState())
{ //will be TRUE if any button changes state (on to off, or off to on)
if (ps2x.Button(PSB_L3))//停止
{
g_carstate = enSTOP;
Serial.println("L3 pressed");
}
if (ps2x.Button(PSB_R3)) //复位
{
front_detection();
Serial.println("R3 pressed");
}
if (ps2x.Button(PSB_L2)) //加速
{
Serial.println("L2 pressed");
control += 50;
if (control > 255)
{
control = 255;
}
}
if (ps2x.Button(PSB_R2))
{
Serial.println("R2 pressed");
control -= 50;
if (control< 50)
{
control = 100;
}
}
if (ps2x.Button(PSB_TRIANGLE)) //点灯
{
Serial.println("Triangle pressed");
digitalWrite(Led, !digitalRead(Led)); //反转电平
}
}
if (ps2x.ButtonPressed(PSB_CIRCLE))//灭火 //will be TRUE if button was JUST pressed
{
Serial.println("Circle just pressed");
digitalWrite(Fire, !digitalRead(Fire)); //反转电平
}
if (ps2x.NewButtonState(PSB_CROSS))//鸣笛 //will be TRUE if button was JUST pressed OR released
{
Serial.println("X just changed");
// whistle();
}
if (ps2x.ButtonReleased(PSB_SQUARE))//唱歌 //will be TRUE if button was JUST released
{
Serial.println("Square just released");
//PlayTest();
}
if (ps2x.Button(PSB_L1) || ps2x.Button(PSB_R1))
{ //print stick values if either is TRUE
Serial.print("Stick Values:");
Serial.print(ps2x.Analog(PSS_LY), DEC); //Left stick, Y axis. Other options: LX, RY, RX
Serial.print(",");
Serial.print(ps2x.Analog(PSS_LX), DEC);
Serial.print(",");
Serial.print(ps2x.Analog(PSS_RY), DEC);
Serial.print(",");
Serial.println(ps2x.Analog(PSS_RX), DEC);
Y1 = ps2x.Analog(PSS_LY);
X1 = ps2x.Analog(PSS_LX);
Y2 = ps2x.Analog(PSS_RY);
X2 = ps2x.Analog(PSS_RX);
/*左摇杆*/
if (Y1< 5&& X1 > 80&& X1< 180) //上
{
g_carstate = enRUN;
}
else if (Y1 > 230&& X1 > 80&& X1< 180) //下
{
g_carstate = enBACK;
}
else if (X1< 5&& Y1 > 80&& Y1< 180) //左
{
g_carstate = enLEFT;
}
else if (Y1 > 80&& Y1< 180&& X1 > 230)//右
{
g_carstate = enRIGHT;
}
else if (Y1< = 80&& X1< = 80) //左上
{
g_carstate = enUPLEFT;
}
else if (Y1< = 80&& X1 >= 180) //右上
{
g_carstate = enUPRIGHT;
}
else if (X1< = 80&& Y1 >= 180) // 左下
{
g_carstate = enDOWNLEFT;
}
else if (Y1 >= 180&& X1 >= 180) //右下
{
g_carstate = enDOWNRIGHT;
}
else//停
{
g_carstate = enSTOP;
}
/*右摇杆*/
if (X2< 5&& Y2 > 110&& Y2< 150) //左
{
g_servostate = 1;
}
else if (Y2 > 110&& Y2< 150&& X2 > 230)//右
{
g_servostate = 2;
}
else//归位
{
g_servostate = 0;
}
}
}