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

For Phase 3 Part 1

parent 387d5be7
......@@ -6,10 +6,11 @@ volatile uint8_t state;
enum States_enum {OFF, ON, STOP, FORWARD, BACKWARD, LEFT, RIGHT, CLOCKWISE, CCLOCKWISE, SEARCH};
volatile uint8_t cmd;
volatile char off;
enum Movements_enum {halt, forward, backward, left, right, rotate_cw, rotate_ccw, search};
/* Pin to turn on/off ground sensor */
const byte sensorLeds = 6;
const byte sensorLeds = 13;
/* Pin for analog input from ground sensor */
const byte analogPin = A0;
......@@ -17,7 +18,7 @@ const byte analogPin = A0;
const int B_LOW = 300;
const int B_HIGH = 500;
const int R_LOW = 500;
const int R_HIGH = 800;
const int R_HIGH = 835;
/* Edit this for bot functionality
* stop = 's'
......@@ -26,10 +27,10 @@ const int R_HIGH = 800;
* right = 'r'
* left = 'l'
*/
const char RED_DO = right;
const char RED_DO = forward;
const char BLUE_DO = forward;
const char BLACK_DO = search;
const char YELLOW_DO = halt;
const char BLACK_DO = forward;
const char YELLOW_DO = forward;
/* Set up the ground sensor */
Bounds bound = {B_LOW, B_HIGH, R_LOW, R_HIGH};
......@@ -64,6 +65,7 @@ MotorControl motors(w, s);
void stateControl(uint8_t c);
void bootSequence();
void detect();
void setup() {
//attachInterrupt(digitalPinToInterrupt(sensorLeds), detect, CHANGE);
......@@ -75,23 +77,25 @@ void setup() {
}
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*/
while (Serial.available()) {
off = Serial.read(); //gets one byte from serial buffer
if ((off == 'Y') || (off == 'y')) {
state = ON;
}
}
digitalWrite(sensorLeds, LOW);
break;
case ON:
bootSequence();
tapeSensor.senseColor(&cmd);
stateControl();
delay(100);
break;
case STOP:
......@@ -139,18 +143,7 @@ void loop() {
case SEARCH:
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);
detect();
break;
}
}
......@@ -170,6 +163,14 @@ void bootSequence()
void stateControl()
{
while (Serial.available()) {
char c = Serial.read(); //gets one byte from serial buffer
if (c) {
state = OFF;
}
delay(5);
}
if (cmd == forward){
Serial.println("Going Forward");
state = FORWARD;
......@@ -199,9 +200,59 @@ void stateControl()
delay(5); //slow loop to allow for change in state
}
/*void detect()
void detect()
{
tapeSensor.senseColor(&cmd);
stateControl();
}*/
uint8_t old_cmd = cmd;
motors.ccw();
delay(200);
motors.halt();
tapeSensor.senseColor(&cmd);
stateControl();
delay(500);
if (old_cmd == cmd) {
motors.forward();
delay(200);
motors.halt();
tapeSensor.senseColor(&cmd);
stateControl();
delay(500);
}
if (old_cmd == cmd) {
motors.cw();
delay(200);
motors.halt();
tapeSensor.senseColor(&cmd);
stateControl();
delay(500);
}
if (old_cmd == cmd) {
motors.forward();
delay(200);
motors.halt();
tapeSensor.senseColor(&cmd);
stateControl();
delay(500);
}
if (old_cmd == cmd) {
motors.cw();
delay(200);
motors.halt();
tapeSensor.senseColor(&cmd);
stateControl();
delay(500);
}
if (old_cmd == cmd) {
motors.forward();
delay(200);
motors.halt();
tapeSensor.senseColor(&cmd);
stateControl();
delay(500);
}
}
#include <GroundSensor.h>
#include "Arduino.h"
const byte sensorleds = 13;
const byte analogPin = A0;
volatile uint8_t cmd;
enum Movements_enum {halt, forward, backward, left, right, rotate_cw, rotate_ccw, search};
// 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 = 835;
/* 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;
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);
void setup() {
Serial.begin(9600);
Serial.println("Ground Sensor Test"); // so I can keep track of what is loaded
Serial.println("Trying to detect color");
cmd = 'f';
}
void loop() {
while (Serial.available()) {
cmd = Serial.read(); //gets one byte from serial buffer
if ((cmd == 'o')) {
digitalWrite(sensorleds, HIGH);
} else if ((cmd == 'f')) {
digitalWrite(sensorleds, LOW);
}
}
tapeSensor.senseColor(&cmd);
}
#include <MotorControl.h>
#include "Arduino.h"
const byte WheelLeftF = 2;
const byte WheelLeftB = 3;
const byte WheelRightF = 4;
const byte WheelRightB = 5;
volatile int curr_speed;
const int MAX = 255;
const int TQUARTER = 191;
const int HALF = 127;
const int QUARTER = 64;
const int ZERO = 0;
String readString;
/* 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);
volatile uint8_t state;
enum States_enum {STOP, FORWARD, BACKWARD, LEFT, RIGHT, CIRCLE};
void stateControl();
void speedControl();
void setup() {
Serial.begin(9600);
Serial.println("Driving motor motion Test"); // so I can keep track of what is loaded
Serial.println("Stop");
state = STOP;
}
void loop() {
switch (state)
{
case STOP:
stateControl();
motors.halt(); // Stop right away
break;
case FORWARD:
stateControl();
motors.forward();
break;
case BACKWARD:
stateControl();
motors.backward();
break;
case LEFT:
stateControl();
motors.left();
break;
case RIGHT:
stateControl();
motors.right();
break;
case CIRCLE:
stateControl();
motors.cw();
break;
}
}
void stateControl()
{
while (Serial.available()) {
char c = Serial.read(); //gets one byte from serial buffer
readString += c; //makes the string readString
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 == 'c'){
Serial.println("Turn 360");
state = CIRCLE;
}
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
}
}
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