So here is my code update. The code above is for using '0' degrees as starting point. Now, If you have a sensor that detects a clear area to drive robot to, denote the degrees, and replace the turn function argument. In this case, I just set it to -90 for testing. works fine with Pos and Neg degrees. This is good for turning a robot a certain amount without motor timers or motor position feedback or SLAM.
#include <QMC5883LCompass.h>
#include <FastLED.h>
#include <Ultrasonic.h>
QMC5883LCompass compass;
int currentHeading = 0;
int initialHeading = 0;
const int LED = 13;
Ultrasonic ultrasonic(2, 20);
int distance;
unsigned long US_Distance_Ping_Timer = 0;
bool turnFlag = true;
//MOTOR A
#define enA 22
#define in1 14
#define in2 15
//Motor B
#define enB 23
#define in3 16
#define in4 17
enum motorDriveStates {
STOP, //stop motors
FORWARD, //drive forward
BACKWARD,
TURN_RIGHT,
TURN_LEFT,
FORWARD_SPEED_UP, //go fro 0 to X
FORWARD_SLOW_DOWN, //go from X to 0
REVERSE_SPEED_UP,
REVERSE_SLOW_DOWN
};
motorDriveStates MOTOR_DRIVE_FUNCTIONS();
int motorPWM = 0;
int motorState = LOW;
const int buttonPin = 4;
int buttonState = 0;
void setup() {
Serial.begin(9600);
compass.init();
compass.setCalibration(-1763, 1522, -2108, 1168, -1640, 557);//adjust for your location
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
MOTOR_DRIVE_FUNCTIONS(0);
pinMode(LED, OUTPUT);
pinMode(buttonPin, INPUT_PULLDOWN);
}
//=====these are the timer items==========
// millis variable used for many functions
unsigned long currentMillis = 0;
//array of subjective milli run times
int MillisRunLengthTime[8] = { 20, 100, 200, 800, 1000, 1500, 2000, 2500 };
//this is the key timer function, can use for MANY functions
boolean RunTIMERforThisAmountOfTime(unsigned long &startNowTimer, int runLengthTime) {
currentMillis = millis();
if (currentMillis - startNowTimer >= runLengthTime) {
startNowTimer = currentMillis;
return true;
} else return false;
}
void loop() {
EVERY_N_MILLISECONDS(100) {//keep track of current heading
int x, y;
compass.read();
x = compass.getX();
y = compass.getY();
float headingRadians = atan2(y, x);
float headingDegrees = headingRadians * 180 / M_PI;
if (headingDegrees < 0) {
headingDegrees += 360;
}
initialHeading = headingDegrees;
//Serial.print("initialHeading Heading: ");
//Serial.println(initialHeading);
}
/*
buttonState = digitalRead(buttonPin); //for debugging
if (buttonState == HIGH) {
digitalWrite(LED, HIGH); //for debugging
turnFlag = true;
turnControl(50);
// MOTOR_DRIVE_FUNCTIONS(FORWARD);
} else {
digitalWrite(LED, LOW);
//MOTOR_DRIVE_FUNCTIONS(STOP);
}
}
*/
if (RunTIMERforThisAmountOfTime(US_Distance_Ping_Timer, MillisRunLengthTime[1])) {
distance = ultrasonic.read();
//Serial.print("Distance in CM: ");
//Serial.println(distance);
}
if (distance <= 25) {//set for your system
turnControl(-90); // substitute any angle
//for turn requirement -90, 40, etc...up to 179+/-
//add a rotating sensor to determine best turn angle
}
if (distance >= 26) {
//turnFlag = true;
MOTOR_DRIVE_FUNCTIONS(FORWARD);
}
}
//==============turn control===============
void turnControl(int angle) {
int initialDirection = initialHeading;//get initial heading
//and normalize 'x' for 0-359
initialDirection %= 360;
if (initialDirection < 0) {
initialDirection += 360;
}
int initialTARGETAngle = initialDirection + angle;//get the initial target angle
//and normalize 'x' for 0-359
initialTARGETAngle %= 360;
if (initialTARGETAngle < 0) {
initialTARGETAngle += 360;
}
int direction;
while (1) {
//need live compass reading while turning, so....
int x, y;
compass.read();
x = compass.getX();
y = compass.getY();
float headingRadians = atan2(y, x);
float headingDegrees = headingRadians * 180 / M_PI;
if (headingDegrees < 0) {
headingDegrees += 360;
}
currentHeading = headingDegrees;
Serial.print("initialDirection: ");
Serial.println(initialDirection);
Serial.print("initialTARGETAngle: ");
Serial.println(initialTARGETAngle);
int diff = initialTARGETAngle - currentHeading;
//TODO: fix the jitter at the 180 transition
if (diff < -180)
diff += 360;
else if (diff > 180)
diff -= 360;
direction = -diff;
if (direction > 0) {
MOTOR_DRIVE_FUNCTIONS(TURN_RIGHT);
} else {
MOTOR_DRIVE_FUNCTIONS(TURN_LEFT);
}
Serial.print("DIFF: ");
Serial.println(diff);
if (abs(diff) < 10) {//a little leeway here as needed
MOTOR_DRIVE_FUNCTIONS(STOP);
return;
}
}
}
//==============MOTOR FUNCTIONS==================
void MOTOR_DRIVE_FUNCTIONS(int var) {
switch (var) {
case 0: //motorSTOP:
analogWrite(enA, 0);
analogWrite(enB, 0);
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
break;
case 1: //motorBothForward:
analogWrite(enA, 220);
analogWrite(enB, 220);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
break;
case 2: //motorBOTHback:
analogWrite(enA, 190);
analogWrite(enB, 190);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
break;
case 3: //motorRIGHTturn:
analogWrite(enA, 220);
analogWrite(enB, 220);
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
break;
case 4: //motorLEFTturn:
analogWrite(enA, 220);
analogWrite(enB, 220);
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
break;
case 5: //motorForwardSpeedUP:
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
for (int i = 0; i < 220; i++) {
//motorPWM = i;
analogWrite(enA, i);
analogWrite(enB, i);
delay(20);
}
break;
case 6: //motorForwardSlowDown:
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, HIGH);
digitalWrite(in4, LOW);
for (int i = 220; i >= 0; --i) {
motorPWM = i;
analogWrite(enA, i);
analogWrite(enB, i);
delay(5);
}
break;
case 7: //motorBackwardSpeedup:
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
for (int i = 0; i < 220; i++) {
motorPWM = i;
analogWrite(enA, i);
analogWrite(enB, i);
delay(5);
}
break;
case 8: //motorBackwardSlowDown:
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
for (int i = 220; i >= 0; --i) {
motorPWM = i;
analogWrite(enA, i);
analogWrite(enB, i);
delay(5);
}
break;
}
}
