Unverified Commit 04d9d900 authored by Luan Banh's avatar Luan Banh Committed by GitHub
Browse files

Moving the bot based on color detected

Wheels are not calibrated. FollowLineUsingLib is untested. Not sure if written libraries work as intended.
parent 6b9147a3
#include <MotorControl.h>
#include <GroundSensor.h>
#include "Arduino.h"
volatile uint8_t state;
enum States_enum {OFF, ON, STOP, FORWARD, BACKWARD, LEFT, RIGHT, CLOCKWISE, CCLOCKWISE, SEARCH};
volatile uint8_t cmd;
enum Movements_enum {halt, forward, backward, left, right, rotate_cw, rotate_ccw, search};
/* Pin to turn on/off ground sensor */
const byte sensorLeds = 6;
/* Pin for analog input from ground sensor */
const byte analogPin = A0;
/* Range of colors are partitions of 1023 */
const int B_LOW = 300;
const int B_HIGH = 500;
const int R_LOW = 500;
const int R_HIGH = 800;
/* Edit this for bot functionality
* stop = 's'
* forward = 'f'
* backward = 'b'
* right = 'r'
* left = 'l'
*/
const char RED_DO = right;
const char BLUE_DO = forward;
const char BLACK_DO = search;
const char YELLOW_DO = halt;
/* Set up the ground sensor */
Bounds bound = {B_LOW, B_HIGH, R_LOW, R_HIGH};
Instructions instr = {RED_DO, BLUE_DO, BLACK_DO, YELLOW_DO};
GroundSensor tapeSensor(sensorLeds, analogPin, bound, instr);
/* Pins for wheel control */
const byte WheelLeftF = 2;
const byte WheelLeftB = 3;
const byte WheelRightF = 4;
const byte WheelRightB = 5;
/* Duty Cycle constants */
const int MAX = 255;
const int TQUARTER = 191;
const int HALF = 127;
const int QUARTER = 64;
const int ZERO = 0;
/* Control speed of each state*/
const int FWD_SPD = QUARTER;
const int BCK_SPD = QUARTER;
const int LFT_SPD = QUARTER;
const int RGT_SPD = QUARTER;
const int RT_CW_SPD = QUARTER;
const int RT_CCW_SPD = QUARTER;
/* Control the motor */
Wheels w = {WheelLeftF, WheelLeftB, WheelRightF, WheelRightB};
Speeds s = {FWD_SPD, BCK_SPD, LFT_SPD, RGT_SPD, RT_CW_SPD, RT_CCW_SPD};
MotorControl motors(w, s);
void stateControl(uint8_t c);
void bootSequence();
void setup() {
attachInterrupt(digitalPinToInterrupt(analogPin), detect, CHANGE);
Serial.begin(9600);
Serial.println("Follow the Line Color"); // so I can keep track of what is loaded
Serial.println("Bot is OFF");
state = OFF;
Serial.println("Turn it on? Y/N");
}
void loop() {
while (Serial.available()) {
char c = Serial.read(); //gets one byte from serial buffer
if ((c = 'Y') || (c = 'y')) {
state = ON;
}
}
switch (state)
{
case OFF:
/* Starting state, wait for user to turn on*/
break;
case ON:
bootSequence();
break;
case STOP:
motors.stop(); // Stop right away
break;
case FORWARD:
stateControl(cmd);
motors.forward();
break;
case BACKWARD:
stateControl(cmd);
motors.backward();
break;
case LEFT:
stateControl(cmd);
motors.left();
break;
case RIGHT:
stateControl(cmd);
motors.right();
break;
case CLOCKWISE:
stateControl(cmd);
motors.cw();
break;
case CCLOCKWISE:
stateControl(cmd);
motors.ccw();
break;
case SEARCH:
stateControl(cmd);
motors.ccw();
delay(1000);
motors.cw();
delay(3000);
break;
}
}
void bootSequence()
{
Serial.println("Bot is on");
Serial.println("Moving in ");
Serial.println("3");
delay(1000);
Serial.println("2");
delay(1000);
Serial.println("1");
delay(1000);
digitalWrite(sensorLeds, HIGH);
tapeSensor.senseColor(&cmd);
}
void stateControl(uint8_t c)
{
if (c == forward){
Serial.println("Going Forward");
state = FORWARD;
} else if (c == backward){
Serial.println("Moving Backward");
state = BACKWARD;
} else if (c == left){
Serial.println("Moving Left");
state = LEFT;
} else if (c == right){
Serial.println("Moving Right");
state = RIGHT;
} else if (c == halt){
Serial.println("Stop");
state = STOP;
} else if (c == rotate_cw){
Serial.println("Turn 360 Clockwise");
state = CLOCKWISE;
} else if (c == rotate_ccw){
Serial.println("Turn 360 Counter Clockwise");
state = CCLOCKWISE;
} else if (c == search){
Serial.println("Searching for path");
state = SEARCH;
}
delay(5); //slow loop to allow for change in state
}
void detect()
{
tapeSensor.senseColor(&cmd);
}
#include "Arduino.h"
const byte WheelLeftF = 2;
const byte WheelLeftB = 3;
const byte WheelRightF = 4;
const byte WheelRightB = 5;
const byte sensorLeds = 6;
const byte analogPin = A0;
// Range of colors are partitions of 1023
const int B_LOW = 300;
const int B_HIGH = 500;
const int R_LOW = 500;
const int R_HIGH = 800;
const int MAX = 255;
const int TQUARTER = 191;
const int HALF = 127;
const int QUARTER = 64;
const int ZERO = 0;
/* Edit this for bot functionality
* stop = 's'
* forward = 'f'
* backward = 'b'
* right = 'r'
* left = 'l'
**********************************/
const char RED_DO = 's';
const char BLUE_DO = 'f';
const char BLACK_DO = 's';
const char YELLOW_DO = 'r';
const int FWD_SPD = QUARTER;
const int BCK_SPD = QUARTER;
const int LFT_SPD = QUARTER;
const int RGT_SPD = QUARTER;
const int RT_CW_SPD = QUARTER;
const int RT_CCW_SPD = QUARTER;
/* *********************************/
String readString;
volatile uint8_t state;
enum States_enum {OFF, ON, STOP, FORWARD, BACKWARD, LEFT, RIGHT, CLOCKWISE, CCLOCKWISE};
void pinClose(byte pin);
void pinOpen(byte pin);
void allPinsOpen();
void stateControl(char c);
void bootSequence();
void driveMotor(byte pinOpen1, byte pinOpen2, byte pinClose1, byte pinClose2);
void setup() {
pinMode(LED_BUILTIN, OUTPUT);
pinMode(WheelLeftF, OUTPUT);
pinMode(WheelLeftB, OUTPUT);
pinMode(WheelRightF, OUTPUT);
pinMode(WheelRightB, OUTPUT);
pinMode(sensorLeds, OUTPUT);
Serial.begin(9600);
Serial.println("Follow the Line Color"); // so I can keep track of what is loaded
Serial.println("OFF");
state = OFF;
}
void loop() {
while (Serial.available()) {
char c = Serial.read(); //gets one byte from serial buffer
readString += c; //makes the string readString
if (c = 'g') {
state = ON;
}
delay(2); //slow looping to allow buffer to fill with next character
}
if (readString.length() >0) {
readString=""; //empty for next input
delay(2); //slow down a bit so motors have time to get inputs
}
switch (state)
{
case OFF:
/* Starting state, nothing should be on */
allPinsOpen();
break;
case ON:
bootSequence();
senseTape();
break;
case STOP:
senseTape();
allPinsOpen();
break;
case FORWARD:
senseTape();
pinOpen(WheelLeftB);
pinOpen(WheelRightB);
analogWrite(WheelLeftF, QUARTER);
analogWrite(WheelRightF, QUARTER);
break;
case BACKWARD:
senseTape();
pinOpen(WheelLeftF);
pinOpen(WheelRightF);
analogWrite(WheelLeftB, QUARTER);
analogWrite(WheelRightB, QUARTER);
break;
case LEFT:
senseTape();
pinOpen(WheelLeftB);
pinOpen(WheelRightB);
analogWrite(WheelLeftF, ZERO);
analogWrite(WheelRightF, QUARTER);
break;
case RIGHT:
senseTape();
pinOpen(WheelLeftB);
pinOpen(WheelRightB);
analogWrite(WheelLeftF, QUARTER);
analogWrite(WheelRightF, ZERO);
break;
case CLOCKWISE:
senseTape();
/*
pinOpen(WheelLeftB);
pinOpen(WheelRightF);
analogWrite(WheelLeftF, QUARTER);
analogWrite(WheelRightB, QUARTER);
*/
driveMotor(WheelLeftB, WheelRightF, WheelLeftF, WheelRightB);
break;
case CCLOCKWISE:
senseTape();
/*
pinOpen(WheelLeftF);
pinOpen(WheelRightB);
analogWrite(WheelLeftB, QUARTER);
analogWrite(WheelRightF, QUARTER);
*/
driveMotor(WheelLeftF, WheelRightB, WheelLeftB, WheelRightF);
break;
}
}
void senseTape()
{
int voltage = analogRead(analogPin);
bool is_black = voltage < B_LOW;
bool is_blue = (voltage >= B_LOW) and (voltage <= B_HIGH);
bool is_red = (voltage > R_LOW) and (voltage <= R_HIGH);
bool is_yellow = voltage > R_HIGH;
char cmd;
if (is_black) {
Serial.println("Detected Black");
cmd = BLACK_DO;
} else if (is_blue) {
Serial.println("Detected Blue");
cmd = BLUE_DO;
} else if (is_red) {
Serial.println("Detected Red");
cmd = RED_DO;
} else if (is_yellow) {
Serial.println("Detected Yellow");
cmd = YELLOW_DO;
} else {
Serial.println("Invalid voltage detected! Check Code!");
cmd = 's';
}
stateControl(cmd);
}
void driveMotor(byte pinOpen1, byte pinOpen2, byte pinClose1, byte pinClose2)
{
pinOpen(pinOpen1);
pinOpen(pinOpen2);
switch (state)
{
case FORWARD:
analogWrite(WheelLeftF, FWD_SPD);
analogWrite(WheelRightF, FWD_SPD);
break;
case BACKWARD:
analogWrite(WheelLeftB, BCK_SPD);
analogWrite(WheelRightB, BCK_SPD);
break;
case LEFT:
analogWrite(WheelLeftF, ZERO);
analogWrite(WheelRightF, LFT_SPD);
break;
case RIGHT:
analogWrite(WheelLeftF, RGT_SPD);
analogWrite(WheelRightF, ZERO);
break;
case CLOCKWISE:
analogWrite(WheelLeftF, RT_CW_SPD);
analogWrite(WheelRightB, RT_CW_SPD);
break;
case CCLOCKWISE:
analogWrite(WheelLeftB, RT_CCW_SPD);
analogWrite(WheelRightF, RT_CCW_SPD);
break;
}
}
void bootSequence()
{
Serial.println("Bot is on");
Serial.println("Moving in ");
Serial.println("3");
delay(1000);
Serial.println("2");
delay(1000);
Serial.println("1");
delay(1000);
pinClose(sensorLeds);
}
void allPinsOpen()
{
pinOpen(WheelLeftB);
pinOpen(WheelRightB);
pinOpen(WheelLeftF);
pinOpen(WheelRightF);
pinOpen(sensorLeds);
}
void stateControl(char c)
{
if (c == 'f'){
Serial.println("Going Forward");
state = FORWARD;
} else if (c == 'b'){
Serial.println("Moving Backward");
state = BACKWARD;
} else if (c == 'l'){
Serial.println("Moving Left");
state = LEFT;
} else if (c == 'r'){
Serial.println("Moving Right");
state = RIGHT;
} else if (c == 's'){
Serial.println("Stop");
state = STOP;
} else if (c == 'd'){
Serial.println("Turn 360 Clockwise");
state = CLOCKWISE;
} else if (c == 'a'){
Serial.println("Turn 360 Counter Clockwise");
state = CCLOCKWISE;
}
delay(5); //slow loop to allow for change in state
}
void pinOpen(byte pin)
{
digitalWrite(pin, LOW);
}
void pinClose(byte pin)
{
digitalWrite(pin, HIGH);
}
#include "Arduino.h"
#include "FourPinControl.h"
FourPinControl::FourPinControl()
{
_p1 = _p2 = _p3 = _p4 = 0;
}
void FourPinControl::newOutputs(int p1, int p2, int p3, int p4)
{
// Set pins to output mode, remember them
pinMode(p1, OUTPUT);
pinMode(p2, OUTPUT);
pinMode(p3, OUTPUT);
pinMode(p4, OUTPUT);
_p1 = p1;
_p2 = p2;
_p3 = p3;
_p4 = p4;
}
/* Opening Pins Section*/
// Open a pin
void FourPinControl::pinOpen(int p)
{
digitalWrite(p, LOW);
}
// Open two pins
void FourPinControl::twoPinsOpen(int p1, int p2)
{
digitalWrite(p1, LOW);
digitalWrite(p2, LOW);
}
// Open three pins
void FourPinControl::threePinsOpen(int p1, int p2, int p3)
{
digitalWrite(p1, LOW);
digitalWrite(p2, LOW);
digitalWrite(p3, LOW);
}
// Open all four pins
void FourPinControl::allPinsOpen()
{
digitalWrite(_p1, LOW);
digitalWrite(_p2, LOW);
digitalWrite(_p3, LOW);
digitalWrite(_p4, LOW);
}
/* Closing Pins Section*/
// Close a pin
void FourPinControl::pinClose(int p)
{
digitalWrite(p, HIGH);
}
// Close two pins
void FourPinControl::twoPinsClose(int p1, int p2)
{
digitalWrite(p1, HIGH);
digitalWrite(p2, HIGH);
}
// Close three pins
void FourPinControl:: threePinsClose(int p1, int p2, int p3)
{
digitalWrite(p1, HIGH);
digitalWrite(p2, HIGH);
digitalWrite(p3, HIGH);
}
// Close all four pins
void FourPinControl::allPinsClose()
{
digitalWrite(_p1, HIGH);
digitalWrite(_p2, HIGH);
digitalWrite(_p3, HIGH);
digitalWrite(_p4, HIGH);
}
\ No newline at end of file
#ifndef FourPinControl_h
#define FourPinControl_h
#include "Arduino.h"
class FourPinControl
{
public:
FourPinControl();
void newOutputs(int p1, int p2, int p3, int p4);
void pinOpen(int p);
void twoPinsOpen(int p1, int p2);
void threePinsOpen(int p1, int p2, int p3);
void allPinsOpen();
void pinClose(int p);
void twoPinsClose(int p1, int p2);
void threePinsClose(int p1, int p2, int p3);
void allPinsClose();
private:
int _p1, _p2, _p3, _p4;
};
#endif
#include "Arduino.h"
#include "GroundSensor.h"
GroundSensor::GroundSensor(int sensorPin, int analogPin,
Bounds b, Instructions i)
{
pinMode(sensorPin, OUTPUT);
_sensorPin = sensorPin;
_analogPin = analogPin;
_bound = b;
_inst = i;
}
void GroundSensor::senseColor(volatile uint8_t *command)
{
int voltage = analogRead(_analogPin);
bool is_black = voltage < _bound.b_low;
bool is_blue = (voltage >= _bound.b_low) and (voltage <= _bound.b_high);
bool is_red = (voltage > _bound.r_low) and (voltage <= _bound.r_high);
bool is_yellow = voltage > _bound.r_high;
if (is_black) {
Serial.println("Detected Black");
*command = _inst.black_do;