It's one of those cheap 4wd skidsteer chassis from China, I built a pair of H-Bridges with TIP122/TIP127 chips. In the video, I just have it run through the pre-programmed moves as a bit of a functionality test. I have no sensors on the unit yet.
And here's the sketch:
/* Brian's TeensyBot sketch. 4wd SkidSteer chinese robot Chassis
Teensy++ microcontroller
Driver's Side Motor IO Pins:
14 - Backwards
15 - Forwards
Passenger Side Motor IO Pins:
25 - Forwards
26 - Backwards
14,15,25 and 26 are PWM capable pins.
*/
void setup() {
pinMode(6, OUTPUT); // Onboard LED
pinMode(14, OUTPUT);
pinMode(15, OUTPUT);
pinMode(25, OUTPUT);
pinMode(26, OUTPUT);
}
void goForward() {
// Assuming allStop was called before this - Maybe I should call it from here, first?
digitalWrite(15, HIGH);
digitalWrite(25, HIGH);
}
void goBackward() {
// Again, assume all stopped before this
digitalWrite(14, HIGH);
digitalWrite(26, HIGH);
}
void allStop() {
digitalWrite(14, LOW);
digitalWrite(15, LOW);
digitalWrite(25, LOW);
digitalWrite(26, LOW);
digitalWrite(6, LOW); // onboard LED
}
void goLeft() {
// Slight left turn
digitalWrite(14, LOW);
digitalWrite(25, LOW);
digitalWrite(26, LOW);
digitalWrite(15, HIGH);
}
void goRight() {
digitalWrite(14, LOW);
digitalWrite(15, LOW);
digitalWrite(26, LOW);
digitalWrite(25, HIGH);
}
void clockwise() {
// Driver's side forward, pass side backward
digitalWrite(14, LOW);
digitalWrite(25, LOW);
digitalWrite(15, HIGH);
digitalWrite(26, HIGH);
}
void counterClockwise() {
digitalWrite(15, LOW);
digitalWrite(26, LOW);
digitalWrite(14, HIGH);
digitalWrite(26, HIGH);
}
void loop() {
allStop();
delay(100);
digitalWrite(6, HIGH);
goForward();
delay(1500);
allStop();
delay(450);
digitalWrite(6, HIGH);
goBackward();
delay(1500);
allStop();
delay(450);
digitalWrite(6, HIGH);
goLeft();
delay(750);
goForward();
delay(750);
allStop();
delay(450);
digitalWrite(6, HIGH);
goRight();
delay(750);
goForward();
delay(750);
allStop();
delay(450);
digitalWrite(6, HIGH);
clockwise();
delay(750);
allStop();
delay(450);
digitalWrite(6, HIGH);
counterClockwise();
delay(750);
allStop();
delay(300);
}