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

Updated files

parent 961c73e3
#include "Arduino.h"
const byte analogPin = A0;
// Need actual data for the value of voltage for each color
const int B_RANGE = 50;
const int R_RANGE = 100;
const int Y_RANGE = 150;
void setup() {
Serial.begin(9600);
Serial.println("AnalogIn Read Test"); // so I can keep track of what is loaded
}
void loop() {
int voltage = analogRead(analogPin);
Serial.println("Voltage Value: ");
Serial.println(voltage);
delay(1000);
}
#include <MotorControl.h>
#include "Arduino.h"
const int MAX = 255;
const int TQUARTER = 191;
const int HALF = 127;
const int QUARTER = 64;
const int ZERO = 0;
/* Time of travel and constant speed value*/
const float distance = 5; // inches
const int BEST= 45;
/* Acceleration time and speed*/
const int ACC_TIME = 100;
const int ACC_SPD = QUARTER;
const byte WheelLeftF = 2;
const byte WheelLeftB = 3;
const byte WheelRightF = 4;
const byte WheelRightB = 5;
/* Control speed of each state*/
const int FWD_SPD = BEST;
const int BCK_SPD = BEST+10;
const int LFT_SPD = BEST;
const int RGT_SPD = BEST;
const int RT_CW_SPD = BEST;
const int RT_CCW_SPD = BEST;
/* 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};
Accel a = {ACC_TIME, ACC_SPD};
MotorControl motors(w, s, a);
volatile uint8_t state;
enum States_enum {STOP, FORWARD, BACKWARD};
void stateControl();
void setup() {
Serial.begin(9600);
Serial.println("Distace Test"); // so I can keep track of what is loaded
Serial.println("Stop");
state = STOP;
}
void loop() {
int time_delay = distance * 1000;
switch (state)
{
case STOP:
stateControl();
motors.halt(); // Stop right away
break;
case FORWARD:
stateControl();
motors.forward();
delay(time_delay);
state = STOP;
break;
case BACKWARD:
stateControl();
motors.backward();
delay(time_delay);
state = STOP;
break;
}
}
void stateControl()
{
while (Serial.available()) {
char c = Serial.read(); //gets one byte from serial buffer
if (c == 'f'){
Serial.println("Going Forward");
state = FORWARD;
} else if (c == 'b'){
Serial.println("Moving Backward");
state = BACKWARD;
} else if (c == 's'){
Serial.println("Stop");
state = STOP;
}
delay(2); //slow looping to allow buffer to fill with next character
}
}
#include "Arduino.h"
String readString;
const byte ledPinBLUE = 6;
void setup() {
Serial.begin(9600);
Serial.println("Driving led using Serial Test"); // so I can keep track of what is loaded
pinMode(ledPinBLUE, OUTPUT);
}
void loop() {
int n;
while (Serial.available()) {
char c = Serial.read(); //gets one byte from serial buffer
readString += c; //makes the string readString
delay(2); //slow looping to allow buffer to fill with next character
}
if (readString.length() >0) {
Serial.println("Input is: ");
Serial.println(readString); //so you can see the captured string
n = readString.toInt(); //convert readString into a number
readString=""; //empty for next input
}
analogWrite(ledPinBLUE, n);
}
#include <MotorControl.h>
#include "Arduino.h"
volatile uint8_t state;
enum States_enum {FORWARD, BACKWARD};
void speedControl();
void setup() {
pinMode(LED_BUILTIN, OUTPUT);
pinMode(ledPinGREEN, OUTPUT);
pinMode(ledPinBLUE, OUTPUT);
Serial.begin(9600);
Serial.println("Driving motor using Serial Test"); // so I can keep track of what is loaded
Serial.println("Moving Forward");
curr_speed = 0;
state = FORWARD;
}
void loop() {
switch (state)
{
case FORWARD:
ledOff(ledPinBLUE);
speedControl();
analogWrite(ledPinGREEN, curr_speed);
break;
case BACKWARD:
ledOff(ledPinGREEN);
speedControl();
analogWrite(ledPinBLUE, curr_speed);
break;
}
}
void speedControl()
{
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;
}
delay(2); //slow looping to allow buffer to fill with next character
}
if ((readString.length() >0)) {
Serial.println("Input is:");
Serial.println(readString); //so you can see the captured string
curr_speed = readString.toInt();
Serial.println("Speed:");
Serial.println(curr_speed);
readString = ""; //empty for next input
}
}
......@@ -2,12 +2,23 @@
#include <GroundSensor.h>
#include "Arduino.h"
/* Duty Cycle constants */
const int MAX = 255;
const int TQUARTER = 191;
const int HALF = 127;
const int QUARTER = 64;
const int ZERO = 0;
const int BEST = 45; // Will need to change when weight of bot change
volatile uint8_t state;
enum States_enum {OFF, ON, STOP, FORWARD, BACKWARD, LEFT, RIGHT, CLOCKWISE, CCLOCKWISE, SEARCH};
enum States_enum {OFF, ON, STOP, FORWARD, BACKWARD, LEFT, RIGHT, CLOCKWISE, CCLOCKWISE};
volatile uint8_t cmd;
volatile char off;
enum Movements_enum {halt, forward, backward, left, right, rotate_cw, rotate_ccw, search};
volatile uint8_t on;
enum Movements_enum {halt, forward, backward, left, right, rotate_cw, rotate_ccw, search};
/******************************* Configuration Section **********************************/
/* Pin to turn on/off ground sensor */
const byte sensorLeds = 13;
......@@ -15,27 +26,23 @@ const byte sensorLeds = 13;
const byte analogPin = A0;
/* Range of colors are partitions of 1023 */
/* R = 3.7 kOhms
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 = forward;
const char BLUE_DO = forward;
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};
Instructions instr = {RED_DO, BLUE_DO, BLACK_DO, YELLOW_DO};
GroundSensor tapeSensor(sensorLeds, analogPin, bound, instr);
*/
/* R = 5.6 kOhms */
const int B_LOW = 400;
const int B_HIGH = 700;
const int R_LOW = 700;
const int R_HIGH = 1000;
/* What bot should do when it sense a color*/
const char RED_DO = halt;
const char BLUE_DO = forward;
const char BLACK_DO = halt;
const char YELLOW_DO = right;
/* Pins for wheel control */
const byte WheelLeftF = 2;
......@@ -43,229 +50,169 @@ 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 FWD_SPD = BEST;
const int BCK_SPD = BEST;
const int LFT_SPD = QUARTER;
const int RGT_SPD = QUARTER;
const int RT_CW_SPD = QUARTER;
const int RT_CCW_SPD = QUARTER;
/* Set transition delay*/
int transition_delay = 100;
/* Acceleration time and speed*/
const int ACC_TIME = 200;
const int ACC_SPD = QUARTER;
/*****************************************************************************************/
/* 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, transition_delay);
/* 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);
const int hallSensor = A1;
const int pedestrianLed = 6;
Accel a = {ACC_TIME, ACC_SPD};
MotorControl motors(w, s, a);
void stateControl(uint8_t c);
void stateControl();
void bootSequence();
void detect();
void setup() {
//attachInterrupt(digitalPinToInterrupt(sensorLeds), detect, CHANGE);
pinMode(pedestrianLed, OUTPUT);
Serial.begin(9600);
Serial.println("Follow the Line Color"); // so I can keep track of what is loaded
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;
on = 'n';
cmd = BLACK_DO;
Serial.println("Turn it on? Y/N");
}
void loop() {
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;
on = Serial.read(); //gets one byte from serial buffer
if ((on == 'y')) {
state = ON;
on = 'n';
delay(2);
break;
}
}
}
motors.halt();
digitalWrite(sensorLeds, LOW);
break;
case ON:
bootSequence();
delay(500);
tapeSensor.senseColor(&cmd);
stateControl();
delay(100);
break;
break;
case STOP:
motors.halt();
tapeSensor.senseColor(&cmd);
stateControl();
motors.halt();
break;
case FORWARD:
if (cmd == backward) {
motors.halt();
delay(2000);
}
motors.forward();
tapeSensor.senseColor(&cmd);
stateControl();
motors.forward();
break;
case BACKWARD:
if (cmd == forward) {
motors.halt();
delay(2000);
}
motors.backward();
tapeSensor.senseColor(&cmd);
stateControl();
motors.backward();
break;
case LEFT:
case LEFT:
motors.left();
tapeSensor.senseColor(&cmd);
stateControl();
motors.left();
break;
case RIGHT:
motors.right();
tapeSensor.senseColor(&cmd);
stateControl();
motors.right();
break;
break;
case CLOCKWISE:
motors.cw();
tapeSensor.senseColor(&cmd);
stateControl();
motors.cw();
break;
case CCLOCKWISE:
tapeSensor.senseColor(&cmd);
stateControl();
motors.ccw();
break;
case SEARCH:
tapeSensor.senseColor(&cmd);
stateControl();
detect();
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);
Serial.println("Bot is on");
digitalWrite(sensorLeds, HIGH);
Serial.println("Ready!");
delay(1000);
Serial.println("Go!");
}
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;
} else if (cmd == backward){
Serial.println("Moving Backward");
state = BACKWARD;
} else if (cmd == left){
Serial.println("Moving Left");
state = LEFT;
} else if (cmd == right){
Serial.println("Moving Right");
state = RIGHT;
} else if (cmd == halt){
Serial.println("Stop");
state = STOP;
} else if (cmd == rotate_cw){
Serial.println("Turn 360 Clockwise");
state = CLOCKWISE;
} else if (cmd == rotate_ccw){
Serial.println("Turn 360 Counter Clockwise");
state = CCLOCKWISE;
} else if (cmd == search){
Serial.println("Searching for path");
state = SEARCH;
}
int person = analogRead(hallSensor);
if (person >= 399) {
Serial.println("Pedestrian detected");
state = STOP;
digitalWrite(pedestrianLed, HIGH);
} else {
digitalWrite(pedestrianLed, LOW);
}
delay(5); //slow loop to allow for change in state
}
void detect()
{
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);
/* Change the state based on cmd*/
if (cmd == forward) {
//Serial.println("Going Forward");
state = FORWARD;
} else if (cmd == backward) {
Serial.println("Moving Backward");
state = BACKWARD;
} else if (cmd == left) {
//Serial.println("Moving Left");
state = LEFT;
} else if (cmd == right) {
//Serial.println("Moving Right");
state = RIGHT;
} else if (cmd == halt) {
//Serial.println("Stop");
state = STOP;
} else if (cmd == rotate_cw) {
//Serial.println("Turn 360 Clockwise");
state = CLOCKWISE;
} else if (cmd == rotate_ccw) {
//Serial.println("Turn 360 Counter Clockwise");
state = CCLOCKWISE;
}
delay(2); //slow loop to allow for change in state
while (Serial.available()) {
on = Serial.read(); //gets one byte from serial buffer
if ((on == 'y')) {
state = OFF;
on = 'n';
delay(2);
break;
}
}
}
#include "Arduino.h"
const byte WheelLeftF = 2;
const byte WheelLeftB = 3;