75 lines
1.5 KiB
C++
75 lines
1.5 KiB
C++
#include <SparkFun_TB6612.h>
|
|
|
|
#include <SoftwareSerial.h>
|
|
|
|
#define AIN1 4
|
|
#define BIN1 8
|
|
#define AIN2 5
|
|
#define BIN2 7
|
|
#define PWMA 11
|
|
#define PWMB 10
|
|
#define STBY 6
|
|
|
|
const int offsetA = 1;
|
|
const int offsetB = -1;
|
|
|
|
SoftwareSerial BTSerial(3, 2); // RX | TX
|
|
String reader;
|
|
Motor motor1 = Motor(AIN1, AIN2, PWMA, offsetA, STBY);
|
|
Motor motor2 = Motor(BIN1, BIN2, PWMB, offsetB, STBY);
|
|
|
|
void setup()
|
|
{
|
|
//Serial.begin(9600);
|
|
//Serial.println("Ready:");
|
|
BTSerial.begin(9600); // HC-05 default speed in AT command more
|
|
brake(motor1, motor2);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
char m1 = '9';
|
|
char m2 = '9';
|
|
//Serial.println("There is "+String(BTSerial.available())+" available");
|
|
if (BTSerial.available()==2) {
|
|
m1 = BTSerial.read();
|
|
m2 = BTSerial.read();
|
|
//Serial.println("Receved: "+String(m1)+";"+String(m2));
|
|
switch (m1) {
|
|
case '0':
|
|
motor1.drive(-255);
|
|
break;
|
|
case '1':
|
|
motor1.drive(-170);
|
|
break;
|
|
case '2':
|
|
motor1.drive(0);
|
|
break;
|
|
case '3':
|
|
motor1.drive(170);
|
|
break;
|
|
case '4':
|
|
motor1.drive(255);
|
|
break;
|
|
}
|
|
switch (m2) {
|
|
case '0':
|
|
motor2.drive(-255);
|
|
break;
|
|
case '1':
|
|
motor2.drive(-170);
|
|
break;
|
|
case '2':
|
|
motor2.drive(0);
|
|
break;
|
|
case '3':
|
|
motor2.drive(170);
|
|
break;
|
|
case '4':
|
|
motor2.drive(255);
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|