114 lines
1.9 KiB
C++
114 lines
1.9 KiB
C++
#define MA1 4
|
|
#define MA2 5
|
|
#define MB1 7
|
|
#define MB2 8
|
|
#define EA 6
|
|
#define EB 9
|
|
|
|
//commented out part was for encoders. And it's incomplete. Looking forward to complete it when I get encoder motors.
|
|
|
|
/*#define ACHA 3
|
|
#define ACHB 11
|
|
#define BCHA 2
|
|
#define BCHB 12
|
|
volatile int32_t a = 0;
|
|
volatile int32_t b = 0;
|
|
*/
|
|
void setup()
|
|
{
|
|
Serial.begin(57600);
|
|
//attachInterrupt(digitalPinToInterrupt(BCHA), BMI,RISING);
|
|
//attachInterrupt(digitalPinToInterrupt(ACHA), AMI,RISING);
|
|
/* add setup code here */
|
|
pinMode(MA1,OUTPUT);
|
|
pinMode(MA2, OUTPUT);
|
|
pinMode(MB1, OUTPUT);
|
|
pinMode(MB2, OUTPUT);
|
|
pinMode(EA, OUTPUT);
|
|
pinMode(EB, OUTPUT);
|
|
|
|
}
|
|
/*
|
|
void BMI()
|
|
{
|
|
a += ((digitalRead(ACHB)) ? 1 : -1);
|
|
}
|
|
|
|
void AMI()
|
|
{
|
|
b += ((digitalRead(BCHB)) ? 1 : -1);
|
|
}
|
|
*/
|
|
int r=0, p=0,lp=0,rp=0;
|
|
void loop()
|
|
{
|
|
if (Serial.available())
|
|
{
|
|
Serial.readStringUntil(',');
|
|
Serial.readStringUntil(',');
|
|
Serial.parseFloat();
|
|
Serial.read();
|
|
p = -5 - Serial.parseFloat();
|
|
p = constrain(p, -40, 40);
|
|
Serial.read();
|
|
r = Serial.parseFloat();
|
|
r = constrain(r, -40, 40);
|
|
Serial.read();
|
|
Serial.read();
|
|
|
|
rp = - r * 6.375 - abs(p) * p * 0.16;
|
|
lp = - r * 6.375 + abs(p) * p * 0.16;
|
|
if (r < 0)
|
|
{
|
|
if (lp > 0)
|
|
{
|
|
digitalWrite(MA1, 0);
|
|
digitalWrite(MA2, 1);
|
|
}
|
|
else
|
|
{
|
|
digitalWrite(MA1, 1);
|
|
digitalWrite(MA2, 0);
|
|
}
|
|
if (rp > 0)
|
|
{
|
|
digitalWrite(MB1, 0);
|
|
digitalWrite(MB2, 1);
|
|
}
|
|
else
|
|
{
|
|
digitalWrite(MB1, 1);
|
|
digitalWrite(MB2, 0);
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
if (lp < 0)
|
|
{
|
|
digitalWrite(MA1, 1);
|
|
digitalWrite(MA2, 0);
|
|
}
|
|
else
|
|
{
|
|
digitalWrite(MA1, 0);
|
|
digitalWrite(MA2, 1);
|
|
}
|
|
if (rp < 0)
|
|
{
|
|
digitalWrite(MB1, 1);
|
|
digitalWrite(MB2, 0);
|
|
}
|
|
else
|
|
{
|
|
digitalWrite(MB1, 0);
|
|
digitalWrite(MB2, 1);
|
|
}
|
|
}
|
|
|
|
|
|
analogWrite(EA, constrain(abs(lp), 80, 255));
|
|
analogWrite(EB, constrain(abs(rp), 35, 255));
|
|
}
|
|
}
|