代码
```
#include
#define LED 2
#define NUM_LEDS 6
CRGB leds[NUM_LEDS];
#define PINX A5
#define PINY A3
#define PINKG A1
#define PINLU 5
#define PINLD 6
#define PINRU 10
#define PINRD 9
int TZ=200;//速度调整
int speedcal=0;
int Direction;//方向原始数据
int correctionXR;//x修正后的X
int correctionXL;//x修正后的X
int spd=255;
unsigned long Speed;
unsigned long KGSG;
void motorprint();
void motorcontrol();
void dealX();//X方向调整
void motorprint()
{
pinMode(PINX, INPUT); //设置引脚为输入模式
pinMode(PINY, INPUT); //设置引脚为输入模式
pinMode(PINKG, INPUT); //设置引脚为输入模式
pinMode(A0, OUTPUT); //设置引脚为输入模式
pinMode(LED, OUTPUT); //设置引脚为输入模式
pinMode(PINLU, OUTPUT); //设置引脚为输出模式
pinMode(PINLD, OUTPUT); //设置引脚为输出模式
pinMode(PINRU, OUTPUT); //设置引脚为输出模式
pinMode(PINRD, OUTPUT); //设置引脚为输出模式
digitalWrite(PINLU,LOW);
digitalWrite(PINLD,LOW);
digitalWrite(PINRU,LOW);
digitalWrite(PINRD,LOW);
}
void motorcontrol()
{
if ((TZ>5)&&(Speed>Direction))
{
speedcal=(Speed-Direction);
analogWrite(PINLU,constrain(Speed,0,254));
analogWrite(PINLD,constrain(Speed,0,254));
analogWrite(PINRU,constrain(speedcal,0,254));
analogWrite(PINRD,constrain(speedcal,0,254));
}
else if (TZ<-5)
{
// speedcal=constrain(map((Speed+Direction),Speed,310,Speed,250),Speed,250);
speedcal=(Speed+Direction);
analogWrite(PINLU,constrain(speedcal,0,254));
analogWrite(PINLD,constrain(speedcal,0,254));
analogWrite(PINRU,constrain(Speed,0,254));
analogWrite(PINRD,constrain(Speed,0,254));
}
else
{
analogWrite(PINLU,constrain(Speed-5,0,254));
analogWrite(PINLD,constrain(Speed-5,0,254));
analogWrite(PINRU,constrain(Speed+1,0,254));
analogWrite(PINRD,constrain(Speed+1,0,254));
}
}
void setup()
{
// Serial.begin(19200);
motorprint();
FastLED.addLeds(leds, NUM_LEDS);
digitalWrite(A0,HIGH);
for(int i=0;i<6;i++)
{
leds[i] = CRGB ( 225,255,255);
}
FastLED.show();
}
void loop()
{
/*Speed =pulseIn(Y, HIGH,80000);
Direction =pulseIn(X, HIGH,80000);
KGSG = pulseIn(KG, HIGH,80000);*/
KGSG = pulseIn(PINKG, HIGH,80000);
if(KGSG<1100)
{
spd=3;
for(int i=0;i<6;i++)
{
leds[i] = CRGB ( 0,0,255);
}
FastLED.show();
}
else if(KGSG<1700)
{
spd=2;
for(int i=0;i<6;i++)
{
leds[i] = CRGB ( 225,0,255);
}
FastLED.show();}
else if(KGSG<2000)
{spd=1;
for(int i=0;i<6;i++)
{
leds[i] = CRGB ( 225,0,0);
}
FastLED.show();
}
else
spd=255;
Speed=(constrain(map(pulseIn(PINY, HIGH,80000),989,1968,0,250), 0, 250)/spd);
Direction =constrain(map(pulseIn(PINX, HIGH,90000),998,1982,-TZ,TZ),-TZ,TZ);
if (Speed<20)
{
Speed=0;
digitalWrite(A0,LOW);
motorcontrol();
}
else
{
digitalWrite(A0,HIGH);
motorcontrol();
}
}
```