Category: robots

https://youtu.be/4bpG65nveMk

Here’s the code:

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>

//SERVO SECTION
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

//for servo 0
#define MIN_PULSE_WIDTH_0 900
#define MAX_PULSE_WIDTH_0 1800

//for servo 1
#define MIN_PULSE_WIDTH_1 900+100
#define MAX_PULSE_WIDTH_1 1800+100

//for servo 2
#define MIN_PULSE_WIDTH_2 900-250
#define MAX_PULSE_WIDTH_2 1800-250

//for servo 3
#define MIN_PULSE_WIDTH_3 900-250
#define MAX_PULSE_WIDTH_3 1800-250

#define FREQUENCY 50
//50 for digital

//DC MOTOR SECTION
//Motor A
const int motorPin1 = 9; // Pin 14 of L293
const int motorPin2 = 10; // Pin 10 of L293
//Motor B
const int motorPin3 = 6; // Pin 7 of L293
const int motorPin4 = 5; // Pin 2 of L293

//Motor C
const int motorPin5 = 14;
const int motorPin6 = 15;

//Motor D
const int motorPin7 = 16;
const int motorPin8 = 17;

void setup() {
Serial.begin(9600);

//DC MOTOR SECTION
pwm.begin();
pwm.setPWMFreq(FREQUENCY); // Analog servos run at ~60 Hz updates

//SERVO STUFF
//Set pins as outputs
pinMode(motorPin1, OUTPUT);
pinMode(motorPin2, OUTPUT);
pinMode(motorPin3, OUTPUT);
pinMode(motorPin4, OUTPUT);

pinMode(motorPin5, OUTPUT);
pinMode(motorPin6, OUTPUT);
pinMode(motorPin7, OUTPUT);
pinMode(motorPin8, OUTPUT);

delay(1000);
}

//SERVO SECTION
void turnServo(int channel, int angle) {
pwm.setPWM(channel, 0, pulseWidth(channel, angle));
}

void turnServosLeft() {
//turn servos to 0
turnServo(0, 0);
turnServo(1, 0);
turnServo(2, 0);
turnServo(3, 0);
delay(2000);
}

void turnServosRight() {
//turn servos to 180
turnServo(0, 180);
turnServo(1, 180);
turnServo(2, 90);
turnServo(3, 90);
delay(2000);
}

void setServosStraight() {
turnServo(0, 90);
turnServo(1, 90);
turnServo(2, 90);
turnServo(3, 90);
delay(1000);
}

int pulseWidth(int channel, int angle)
{
int MY_MIN_PULSE_WIDTH;
int MY_MAX_PULSE_WIDTH;

if(channel ==0) {
MY_MIN_PULSE_WIDTH=MIN_PULSE_WIDTH_0;
MY_MAX_PULSE_WIDTH=MAX_PULSE_WIDTH_0;
}
else if(channel == 1) {
MY_MIN_PULSE_WIDTH=MIN_PULSE_WIDTH_1;
MY_MAX_PULSE_WIDTH=MAX_PULSE_WIDTH_1;
}
else if(channel == 2) {
MY_MIN_PULSE_WIDTH=MIN_PULSE_WIDTH_2;
MY_MAX_PULSE_WIDTH=MAX_PULSE_WIDTH_2;
}
else if(channel == 3) {
MY_MIN_PULSE_WIDTH=MIN_PULSE_WIDTH_3;
MY_MAX_PULSE_WIDTH=MAX_PULSE_WIDTH_3;
}

int pulse_wide, analog_value;
pulse_wide = map(angle, 0, 180, MY_MIN_PULSE_WIDTH, MY_MAX_PULSE_WIDTH);

analog_value = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);
Serial.println(analog_value);
return analog_value;
}

//END SERVO SECTION

//DC MOTOR SECTION
void goForwardWithMotorA() {
digitalWrite(motorPin1, LOW);
digitalWrite(motorPin2, HIGH);
}

void goForwardWithMotorB() {
digitalWrite(motorPin3, LOW);
digitalWrite(motorPin4, HIGH);
}

void goForwardWithMotorC() {
digitalWrite(motorPin5, LOW);
digitalWrite(motorPin6, HIGH);
}

void goForwardWithMotorD() {
digitalWrite(motorPin7, LOW);
digitalWrite(motorPin8, HIGH);
}

void goBackWithMotorA() {
digitalWrite(motorPin1, HIGH);
digitalWrite(motorPin2, LOW);
}

void goBackWithMotorB() {
digitalWrite(motorPin3, HIGH);
digitalWrite(motorPin4, LOW);
}

void goBackWithMotorC() {
digitalWrite(motorPin5, HIGH);
digitalWrite(motorPin6, LOW);
}

void goBackWithMotorD() {
digitalWrite(motorPin7, HIGH);
digitalWrite(motorPin8, LOW);
}

void goForwardWithDCMotors() {
//motor a
goForwardWithMotorA();

//motor b
goForwardWithMotorB();

//motor c
goForwardWithMotorC();

//motor d
goForwardWithMotorD();

}

void goBackWithDCMotors() {
//motor a
goBackWithMotorA();

//motor b
goBackWithMotorB();

//motor c
goBackWithMotorC();

//motor d
goBackWithMotorD();
}

void stopDCMotors() {

analogWrite(motorPin1, 0);
analogWrite(motorPin2, 0);
analogWrite(motorPin3, 0);
analogWrite(motorPin4, 0);

analogWrite(motorPin5, 0);
analogWrite(motorPin6, 0);
analogWrite(motorPin7, 0);
analogWrite(motorPin8, 0);
}

void rightSideDCMotorsBackward() {
digitalWrite(motorPin1, HIGH);
digitalWrite(motorPin2, LOW);
digitalWrite(motorPin3, HIGH);
digitalWrite(motorPin4, LOW);
}

void leftSideDCMotorsBackward() {
digitalWrite(motorPin1, HIGH);
digitalWrite(motorPin2, LOW);
digitalWrite(motorPin3, HIGH);
digitalWrite(motorPin4, LOW);
}

//END DC MOTOR SECTION

void loop() {

setServosStraight();

goForwardWithDCMotors();

delay(2000);

stopDCMotors();

turnServosLeft();

}