Unverified Commit 387d5be7 authored by Luan Banh's avatar Luan Banh Committed by GitHub
Browse files

Lib should be working, everything else is a NoGo

parent 04d9d900
......@@ -36,7 +36,6 @@ 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;
......@@ -67,7 +66,7 @@ void stateControl(uint8_t c);
void bootSequence();
void setup() {
attachInterrupt(digitalPinToInterrupt(analogPin), detect, CHANGE);
//attachInterrupt(digitalPinToInterrupt(sensorLeds), 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");
......@@ -91,48 +90,67 @@ void loop() {
case ON:
bootSequence();
tapeSensor.senseColor(&cmd);
stateControl();
break;
case STOP:
motors.stop(); // Stop right away
tapeSensor.senseColor(&cmd);
stateControl();
motors.halt();
break;
case FORWARD:
stateControl(cmd);
tapeSensor.senseColor(&cmd);
stateControl();
motors.forward();
break;
case BACKWARD:
stateControl(cmd);
tapeSensor.senseColor(&cmd);
stateControl();
motors.backward();
break;
case LEFT:
stateControl(cmd);
tapeSensor.senseColor(&cmd);
stateControl();
motors.left();
break;
case RIGHT:
stateControl(cmd);
tapeSensor.senseColor(&cmd);
stateControl();
motors.right();
break;
case CLOCKWISE:
stateControl(cmd);
tapeSensor.senseColor(&cmd);
stateControl();
motors.cw();
break;
case CCLOCKWISE:
stateControl(cmd);
tapeSensor.senseColor(&cmd);
stateControl();
motors.ccw();
break;
case SEARCH:
stateControl(cmd);
tapeSensor.senseColor(&cmd);
stateControl();
motors.ccw();
delay(1000);
motors.halt();
tapeSensor.senseColor(&cmd);
stateControl();
delay(500);
motors.cw();
delay(3000);
tapeSensor.senseColor(&cmd);
stateControl();
motors.halt();
delay(500);
break;
}
}
......@@ -148,33 +166,32 @@ void bootSequence()
Serial.println("1");
delay(1000);
digitalWrite(sensorLeds, HIGH);
tapeSensor.senseColor(&cmd);
}
void stateControl(uint8_t c)
void stateControl()
{
if (c == forward){
if (cmd == forward){
Serial.println("Going Forward");
state = FORWARD;
} else if (c == backward){
} else if (cmd == backward){
Serial.println("Moving Backward");
state = BACKWARD;
} else if (c == left){
} else if (cmd == left){
Serial.println("Moving Left");
state = LEFT;
} else if (c == right){
} else if (cmd == right){
Serial.println("Moving Right");
state = RIGHT;
} else if (c == halt){
} else if (cmd == halt){
Serial.println("Stop");
state = STOP;
} else if (c == rotate_cw){
} else if (cmd == rotate_cw){
Serial.println("Turn 360 Clockwise");
state = CLOCKWISE;
} else if (c == rotate_ccw){
} else if (cmd == rotate_ccw){
Serial.println("Turn 360 Counter Clockwise");
state = CCLOCKWISE;
} else if (c == search){
} else if (cmd == search){
Serial.println("Searching for path");
state = SEARCH;
}
......@@ -182,8 +199,9 @@ void stateControl(uint8_t c)
delay(5); //slow loop to allow for change in state
}
void detect()
/*void detect()
{
tapeSensor.senseColor(&cmd);
}
tapeSensor.senseColor(&cmd);
stateControl();
}*/
......@@ -7,7 +7,7 @@ FourPinControl::FourPinControl()
_p1 = _p2 = _p3 = _p4 = 0;
}
void FourPinControl::newOutputs(int p1, int p2, int p3, int p4)
FourPinControl::FourPinControl(int p1, int p2, int p3, int p4)
{
// Set pins to output mode, remember them
pinMode(p1, OUTPUT);
......
......@@ -7,7 +7,7 @@ class FourPinControl
{
public:
FourPinControl();
void newOutputs(int p1, int p2, int p3, int p4);
FourPinControl(int p1, int p2, int p3, int p4);
void pinOpen(int p);
void twoPinsOpen(int p1, int p2);
......
......@@ -13,14 +13,25 @@ enum Movements_enum {FORWARD, BACK, LEFT, RIGHT, CW, CCW};
MotorControl::MotorControl(Wheels w, Speeds s)
{
_pinControl.newOutputs(w.leftF, w.leftB, w.rightF, w.rightB);
pinMode(w.leftF, OUTPUT);
pinMode(w.leftB, OUTPUT);
pinMode(w.rightF, OUTPUT);
pinMode(w.rightB, OUTPUT);
_w = w;
_s = s;
}
void MotorControl::stop()
void MotorControl::allPinsOpen()
{
_pinControl.allPinsOpen();
digitalWrite(_w.leftF, LOW);
digitalWrite(_w.leftB, LOW);
digitalWrite(_w.rightF, LOW);
digitalWrite(_w.rightB, LOW);
}
void MotorControl::halt()
{
allPinsOpen();
}
void MotorControl::forward()
......@@ -60,7 +71,8 @@ void MotorControl::ccw()
void MotorControl::driveMotor(int movement)
{
_pinControl.twoPinsOpen(_m.w1, _m.w2);
digitalWrite(_m.w1, LOW);
digitalWrite(_m.w2, LOW);
switch (movement)
{
......
......@@ -2,7 +2,6 @@
#define MotorControl_h
#include "Arduino.h"
#include "FourPinControl.h"
struct Wheels {
int leftF, leftB, rightF, rightB;
......@@ -16,7 +15,7 @@ class MotorControl
{
public:
MotorControl(Wheels w, Speeds s);
void stop();
void halt();
void forward();
void backward();
void left();
......@@ -26,15 +25,15 @@ class MotorControl
private:
struct Movement {
int w1, w2, w3, w4;
int w1, w2, w3, w4;
};
Wheels _w;
Speeds _s;
Movement _m;
FourPinControl _pinControl;
void driveMotor(int movement);
void allPinsOpen();
};
#endif
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment