I was using another shield and tried to get it working on the moto4. I know I went all bad and way over head… trying to use current sense and sonar to run and stop the bott from getting stuck.
#include <WickedMotorShield.h>
#include <Servo.h> //add Servo Motor library
#include <NewPing.h> //add Ultrasonic sensor library
#define TRIG_PIN A0 // Pin A0 on the Motor Drive Shield soldered to the ultrasonic sensor
#define ECHO_PIN A1 // Pin A1 on the Motor Drive Shield soldered to the ultrasonic sensor
#define MAX_DISTANCE 300 // sets maximum useable sensor measuring distance to 300cm
#define MAX_SPEED 160 // sets speed of DC traction motors to 150/250 or about 70% of full speed - to get power drain down.
#define MAX_SPEED_OFFSET 40 // this sets offset to allow for differences between the two DC traction motors
#define COLL_DIST 30 // sets distance at which robot stops and reverses to 30cm
#define TURN_DIST COLL_DIST+20 // sets distance at which robot veers away from object
#define RING_BUFFER_DEPTH 16
#define NUM_MOTORS 4
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE); // sets up sensor library to use the correct pins to measure distance.
Wicked_DCMotor leftMotor1(1, MOTOR12_1KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency
Wicked_DCMotor leftMotor2(2, MOTOR12_1KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency
Wicked_DCMotor rightMotor1(3, MOTOR34_1KHZ);// create motor #3, using M3 output, set to 1kHz PWM frequency
Wicked_DCMotor rightMotor2(4, MOTOR34_1KHZ);// create motor #4, using M4 output, set to 1kHz PWM frequency
Servo myservo; // create servo object to control a servo
Wicked_DCMotor *m[] = {&motor1, &motor2, &motor3, &motor4};
const char * m_headings[] = {“M1”, “M2”, “M3”, “M4”};
/*
#define NUM_MOTORS 6
Wicked_DCMotor motor1(M1);
Wicked_DCMotor motor2(M2);
Wicked_DCMotor motor3(M3);
Wicked_DCMotor motor4(M4);
Wicked_DCMotor motor5(M5);
Wicked_DCMotor motor6(M6);
Wicked_DCMotor *m[] = {&motor1, &motor2, &motor3, &motor4, &motor5, &motor6};
const char * m_headings[] = {“M1”, “M2”, “M3”, “M4”, “M5”, “M6”};
*/
uint16_t ring_buffers[NUM_MOTORS][RING_BUFFER_DEPTH];
uint8_t ring_buffer_index;
uint32_t ringBufferAverage(uint8_t index);
int leftDistance, rightDistance; //distances on either side
int curDist = 0;
String motorSet = “”;
int speedSet = 0;
//-------------------------------------------- SETUP LOOP ----------------------------------------------------------------------------
void setup() {
myservo.attach(10); // attaches the servo on pin 10 (SERVO_1 on the Motor Drive Shield to the servo object
myservo.write(90); // tells the servo to position at 90-degrees ie. facing forward.
delay(1000); // delay for one seconds
}
//------------------------------------------------------------------------------------------------------------------------------------
//---------------------------------------------MAIN LOOP ------------------------------------------------------------------------------
void loop() {
myservo.write(90); // move eyes forward
delay(90);
curDist = readPing(); // read distance
if (curDist < COLL_DIST) {changePath();} // if forward is blocked change direction
moveForward(); // move forward
delay(500);
}
//-------------------------------------------------------------------------------------------------------------------------------------
void changePath() {
moveStop(); // stop forward movement
myservo.write(36); // check distance to the right
delay(500);
rightDistance = readPing(); //set right distance
delay(500);
myservo.write(144); // check distace to the left
delay(700);
leftDistance = readPing(); //set left distance
delay(500);
myservo.write(90); //return to center
delay(100);
compareDistance();
}
void compareDistance() // find the longest distance
{
if (leftDistance>rightDistance) //if left is less obstructed
{
turnLeft();
}
else if (rightDistance>leftDistance) //if right is less obstructed
{
turnRight();
}
else //if they are equally obstructed
{
turnAround();
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
int readPing() { // read the ultrasonic sensor distance
delay(70);
unsigned int uS = sonar.ping();
int cm = uS/US_ROUNDTRIP_CM;
return cm;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveStop() {leftMotor1.run(RELEASE); leftMotor2.run(RELEASE); rightMotor1.run(RELEASE); rightMotor2.run(RELEASE);} // stop the motors.
//-------------------------------------------------------------------------------------------------------------------------------------
void moveForward() {
motorSet = “FORWARD”;
leftMotor1.run(FORWARD); // turn it on going forward
leftMotor2.run(FORWARD); // turn it on going forward
rightMotor1.run(FORWARD); // turn it on going forward
rightMotor2.run(FORWARD); // turn it on going forward
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
leftMotor1.setSpeed(speedSet);
leftMotor2.setSpeed(speedSet);
rightMotor1.setSpeed(speedSet);
rightMotor2.setSpeed(speedSet);
delay(5);
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
void moveBackward() {
motorSet = “BACKWARD”;
leftMotor1.run(BACKWARD); // turn it on going backward
leftMotor2.run(BACKWARD); // turn it on going backward
rightMotor1.run(BACKWARD); // turn it on going backward
rightMotor2.run(BACKWARD); // turn it on going backward
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
leftMotor1.setSpeed(speedSet);
leftMotor2.setSpeed(speedSet);
rightMotor1.setSpeed(speedSet);
rightMotor2.setSpeed(speedSet);
delay(5);
}
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnRight() {
motorSet = “RIGHT”;
leftMotor1.run(FORWARD); // turn motor 1 forward
leftMotor2.run(FORWARD); // turn motor 2 forward
rightMotor1.run(BACKWARD); // turn motor 3 backward
rightMotor2.run(BACKWARD); // turn motor 4 backward
rightMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
rightMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
delay(1500); // run motors this way for 1500
motorSet = “FORWARD”;
leftMotor1.run(FORWARD); // set both motors back to forward
leftMotor2.run(FORWARD);
rightMotor1.run(FORWARD);
rightMotor2.run(FORWARD);
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnLeft() {
motorSet = “LEFT”;
leftMotor1.run(BACKWARD); // turn motor 1 backward
leftMotor2.run(BACKWARD); // turn motor 2 backward
leftMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
leftMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
rightMotor1.run(FORWARD); // turn motor 3 forward
rightMotor2.run(FORWARD); // turn motor 4 forward
delay(1500); // run motors this way for 1500
motorSet = “FORWARD”;
leftMotor1.run(FORWARD); // turn it on going forward
leftMotor2.run(FORWARD); // turn it on going forward
rightMotor1.run(FORWARD); // turn it on going forward
rightMotor2.run(FORWARD); // turn it on going forward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnAround() {
motorSet = “RIGHT”;
leftMotor1.run(FORWARD); // turn motor 1 forward
leftMotor2.run(FORWARD); // turn motor 2 forward
rightMotor1.run(BACKWARD); // turn motor 3 backward
rightMotor2.run(BACKWARD); // turn motor 4 backward
rightMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
rightMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
delay(1700); // run motors this way for 1700
motorSet = “FORWARD”;
leftMotor1.run(FORWARD); // set both motors back to forward
leftMotor2.run(FORWARD);
rightMotor1.run(FORWARD);
rightMotor2.run(FORWARD);
}