Lab 1
Internal LED Blink
// the setup function runs once when you press reset or power the board
void setup() {
// initialize digital pin LED_BUILTIN as an output.
pinMode(LED_BUILTIN, OUTPUT);
}
// the loop function runs over and over again forever
void loop() {
digitalWrite(LED_BUILTIN, HIGH); // turn the LED on (HIGH is the voltage level)
delay(1000); // wait for a second
digitalWrite(LED_BUILTIN, LOW); // turn the LED off by making the voltage LOW
delay(1000); // wait for a second
}
External LED Blink
// the setup function runs once when you press reset or power the board
int led_pin = 10; //pin connected to external LED
void setup() {
// initialize digital pin LED_BUILTIN as an output.
pinMode(led_pin, OUTPUT);
}
// the loop function runs over and over again forever
void loop() {
digitalWrite(led_pin, HIGH); // turn the LED on (HIGH is the voltage level)
delay(1000); // wait for a second
digitalWrite(led_pin, LOW); // turn the LED off by making the voltage LOW
delay(1000); // wait for a second
}
Potentiometer read
int analog_pin = A0;
void setup() {
// initialize serial communication at 9600 bits per second:
Serial.begin(9600);
}
void loop() {
// read the input pin:
value_analogRead = analogRead(analog_pin);
Serial.println(Value_analogRead);
delay(10); // delay in between reads for stability
}
Analog output, PWM
#include < Servo.h >
int MAX_READING = 1023;
int analog_pin = A0;
int led_pin = 11;
void start_now() {
Serial.begin(9600);
pinMode(analog_pin, INPUT);
pinMode(led_pin, OUTPUT);
}
void setup() {
start_now();
}
void loop() {
int value_analogRead = analogRead(analog_pin);
Serial.print(value_analogRead*5.0/MAX_READING);
Serial.println(" Volts");
output_to_led(value_analogRead);
}
void output_to_led(int value) {
int new_value=map(value,0,MAX_READING,0,255);
analogWrite(led_pin,new_value);
}
Parallax servos
#include < Servo.h >
int MAX_READING = 1023;
int analog_pin = A0;
int led_pin = 11;
int Servo_pin = 5;
Servo MyServo;
void start_now() {
Serial.begin(9600);
pinMode(analog_pin, INPUT);
pinMode(led_pin, OUTPUT);
MyServo.attach(Servo_pin);
}
void setup() {
start_now();
}
void loop() {
int value_analogRead = analogRead(analog_pin);
Serial.print(value_analogRead*5.0/MAX_READING);
Serial.println(" Volts");
output_to_led(value_analogRead);
move_motor(MyServo, value_analogRead);
}
void output_to_led(int value) {
int new_value=map(value,0,MAX_READING,0,255);
analogWrite(led_pin,new_value);
}
void move_motor(Servo motor, int value){
int new_value=map(value,0,MAX_READING,0,255);
motor.write(new_value);
}
Navigation in a square shape
#include < Servo.h >
int MAX_READING = 1023;
int analog_pin = A0;
int led_pin = 11;
int leftServo_pin = 5;
int rightServo_pin = 6;
Servo leftServo;
Servo rightServo;
void start_now() {
Serial.begin(9600);
pinMode(analog_pin, INPUT);
pinMode(led_pin, OUTPUT);
leftServo.attach(leftServo_pin);
rightServo.attach(rightServo_pin); //check pin
}
void setup() {
start_now();
}
void loop() {
int motor_value = 180;
move_motor(leftServo, 180);
move_motor(rightServo, 180);
delay(635);
move_forward();
delay(1000);
}
void output_to_led(int value) {
int new_value=map(value,0,MAX_READING,0,255); //scale to PWM range
analogWrite(led_pin,new_value);
}
void move_motor(Servo motor, int value){
motor.write(value);
}
void move_forward(){
move_motor(leftServo, 0);
move_motor(rightServo,180);
}
Extra Step
#include "arduinoFFT.h"
arduinoFFT FFT = arduinoFFT();
#include < Servo.h >
Servo leftServo;
Servo rightServo;
#define CHANNEL A0
const uint16_t samples = 128;
const double samplingFrequency = 9000; //sample rate
unsigned int sampling_period_us;
unsigned long microseconds;
double vReal[samples];
double vImag[samples];
#define SCL_INDEX 0x00
#define SCL_TIME 0x01
#define SCL_FREQUENCY 0x02
#define SCL_PLOT 0x03
void setup()
{
int leftServo_pin = 5;
int rightServo_pin = 6;
sampling_period_us = round(1000000 * (1.0 / samplingFrequency));
Serial.begin(115200);
}
void loop()
{
leftServo.detach(); //Detach the servos so that the PPM interrupt doesnt interfere with sampling
rightServo.detach();
delay(30);
for (int i = 0; i < samples; i++)
{
microseconds = micros();
vReal[i] = analogRead(A0);
vImag[i] = 0;
while (micros() < (microseconds + sampling_period_us)) {
}
}
leftServo.attach(9); //reattach the servos
rightServo.attach(10);
FFT.Windowing(vReal, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD); //Weigh data
FFT.Compute(vReal, vImag, samples, FFT_FORWARD); //Compute FFT
FFT.ComplexToMagnitude(vReal, vImag, samples); //Compute magnitudes
delay(10);
double f = FFT.MajorPeak(vReal, samples, samplingFrequency); //find the dominant frequency
int mag = vReal[(int)(f * samples / samplingFrequency)]; //find the magnitude of dominant frequency
if (mag > 300) { // filter out lower volume frequencies. That way someone talking won't make it turn right
if (f < 1300) {
turn_right(); // if whistle frequecny is lower than 1300 hz, turn right
}
else if (f > 1900) { //if whistle frequency is higher than 1900 hz, turn left
turn_left();
}
else { // if whistle frequency is somehwere in between, go straight
go_forward();
}
}
else { //if there is no whistle, stop
stop_now();
}
delay(20);
}
void move_motor(Servo motor, int value) {
motor.write(value);
}
void go_forward() {
move_motor(leftServo, 0);
move_motor(rightServo, 180);
}
void turn_right() {
move_motor(leftServo, 180);
move_motor(rightServo, 180);
}
void turn_left() {
move_motor(leftServo, 0);
move_motor(rightServo, 0);
}
void stop_now() {
move_motor(leftServo, 95);
move_motor(rightServo, 90);
}
Lab 2
IR Sensor
#include "arduinoFFT.h"
arduinoFFT FFT = arduinoFFT(); /* Create FFT object */
/*
These values can be changed in order to evaluate the functions
*/
#define CHANNEL A0
const uint16_t samples = 128; //This value MUST ALWAYS be a power of 2
const double samplingFrequency = 40000; //Hz, must be less than 10000 due to ADC
unsigned int sampling_period_us;
unsigned long microseconds;
/*
These are the input and output vectors
Input vectors receive computed results from FFT
*/
double vReal[samples];
double vImag[samples];
#define SCL_INDEX 0x00
#define SCL_TIME 0x01
#define SCL_FREQUENCY 0x02
#define SCL_PLOT 0x03
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
void setup()
{
sampling_period_us = round(1000000*(1.0/samplingFrequency));
Serial.begin(115200);
Serial.println("Ready");
sbi(ADCSRA,ADPS2) ;
cbi(ADCSRA,ADPS1) ;
cbi(ADCSRA,ADPS0) ;
}
void loop()
{
/*SAMPLING*/
for(int i=0; i < samples; i++)
{
microseconds = micros(); //Overflows after around 70 minutes!
vReal[i] = analogRead(CHANNEL);
vImag[i] = 0;
while(micros() < (microseconds + sampling_period_us)){
//empty loop
}
}
/* Print the results of the sampling according to time */
FFT.Windowing(vReal, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD); /* Weigh data */
FFT.Compute(vReal, vImag, samples, FFT_FORWARD); /* Compute FFT */
FFT.ComplexToMagnitude(vReal, vImag, samples); /* Compute magnitudes */
double x = FFT.MajorPeak(vReal, samples, samplingFrequency);
Serial.println(x, 6); //Print out what frequency is the most dominant.
int mag = vReal[(int)(x * samples / samplingFrequency)]; //find the magnitude of dominant frequency
Serial.println(mag);
delay(200); /* Repeat after delay */
}
void PrintVector(double *vData, uint16_t bufferSize, uint8_t scaleType)
{
for (uint16_t i = 0; i < bufferSize; i++)
{
double abscissa;
/* Print abscissa value */
switch (scaleType)
{
case SCL_INDEX:
abscissa = (i * 1.0);
break;
case SCL_TIME:
abscissa = ((i * 1.0) / samplingFrequency);
break;
case SCL_FREQUENCY:
abscissa = ((i * 1.0 * samplingFrequency) / samples);
break;
}
Serial.print(abscissa, 6);
if(scaleType==SCL_FREQUENCY)
Serial.print("Hz");
Serial.print(" ");
Serial.println(vData[i], 4);
}
Serial.println();
}
Microphone
#include "arduinoFFT.h"
arduinoFFT FFT = arduinoFFT(); /* Create FFT object */
#define CHANNEL A0
const uint16_t samples = 128;
const double samplingFrequency = 4000; //sample rate
unsigned int sampling_period_us;
unsigned long microseconds;
double vReal[samples];
double vImag[samples];
#define SCL_INDEX 0x00
#define SCL_TIME 0x01
#define SCL_FREQUENCY 0x02
#define SCL_PLOT 0x03
double f;
int mag;
void setup()
{
sampling_period_us = round(1000000 * (1.0 / samplingFrequency));
Serial.begin(115200);
Serial.println("Hello 2300");
}
void loop()
{
sample();
compute();
if (mag > 1000 && f > 670 && f < 690) { //soft filter
Serial.print("Freq: ");
Serial.println(f, 6);
Serial.print("Mag: ");
Serial.println(mag);
}
else {
Serial.println("none");
}
delay(20);
}
void sample(){
for (int i = 0; i < samples; i++)
{
microseconds = micros();
vReal[i] = analogRead(A0);
vImag[i] = 0;
while (micros() < (microseconds + sampling_period_us)) {
}
}
}
void compute(){
FFT.Windowing(vReal, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD); /* Weigh data */
FFT.Compute(vReal, vImag, samples, FFT_FORWARD); /* Compute FFT */
FFT.ComplexToMagnitude(vReal, vImag, samples); /* Compute magnitudes */
f = FFT.MajorPeak(vReal, samples, samplingFrequency); //find the dominant frequency
mag = vReal[(int)(f * samples / samplingFrequency)]; //find the magnitude of dominant frequency
}
Lab 3
Partial Integration
#include < SPI.h >
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"
#include "Math.h"
//
// Hardware configuration
//
// Set up nRF24L01 radio on SPI bus plus pins 9 & 10
RF24 radio(9,10);
// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipes[2] = { 0x0000000026LL, 0x0000000027LL };
//
// Role management
//
// Set up role. This sketch uses the same software for all the nodes
// in this system. Doing so greatly simplifies testing.
//
// The various roles supported by this sketch
typedef enum { role_ping_out = 1, role_pong_back } role_e;
// The debug-friendly names of those roles
const char* role_friendly_name[] = { "invalid", "Ping out", "Pong back"};
// The role of the current running sketch
role_e role = role_pong_back;
int rows = 2; //change grid size depending on maze
int cols = 3;
byte grid[3][2];
void setup() {
//
// Print preamble
//
Serial.begin(57600);
printf_begin();
//
// Setup and configure rf radio
//
radio.begin();
// optionally, increase the delay between retries & # of retries
radio.setRetries(15,15);
radio.setAutoAck(true);
// set the channel
radio.setChannel(0x50);
// set the power
// RF24_PA_MIN=-18dBm, RF24_PA_LOW=-12dBm, RF24_PA_MED=-6dBM, and RF24_PA_HIGH=0dBm.
radio.setPALevel(RF24_PA_MIN);
//RF24_250KBPS for 250kbs, RF24_1MBPS for 1Mbps, or RF24_2MBPS for 2Mbps
radio.setDataRate(RF24_250KBPS);
// optionally, reduce the payload size. seems to
// improve reliability
//radio.setPayloadSize(8);
//
// Open pipes to other nodes for communication
//
// This simple sketch opens two pipes for these two nodes to communicate
// back and forth.
// Open 'our' pipe for writing
// Open the 'other' pipe for reading, in position #1 (we can have up to 5 pipes open for reading)
if ( role == role_ping_out )
{
radio.openWritingPipe(pipes[0]);
radio.openReadingPipe(1,pipes[1]);
}
else
{
radio.openWritingPipe(pipes[1]);
radio.openReadingPipe(1,pipes[0]);
}
//
// Start listening
//
radio.startListening();
//
// Dump the configuration of the rf unit for debugging
//
radio.printDetails();
}
void loop() {
if ( radio.available() )
{
// Dump the payloads until we've gotten everything
unsigned long got_time;
bool done = false;
while (!done)
{
// Fetch the payload, and see if this was the last one.
done = radio.read( &got_time, sizeof(unsigned long) );
// Spew it
//("Got payload %lu...",got_time);
parse_message(got_time);
// Delay just a little bit to let the other unit
// make the transition to receiver
}
delayMicroseconds(10);
}
void parse_message(unsigned long msg){
byte x = (msg >> 3*8);
byte y = (msg >> 2*8);
byte info = (msg >> 1*8);
grid[x][y] = info;
print_toGUI(x, y, info);
}
void print_toGUI(byte x, byte y, byte info){
bool east = info & 1;
bool north = (info >> 1) & 1;
bool west = (info >> 2) & 1;
bool south = (info >> 3) & 1;
/* Serial.print(x); Serial.print(","); Serial.print(y);
Serial.print(",east="); Serial.print(east? "true" : "false");
Serial.print(",north="); Serial.print(north? "true" : "false");
Serial.print(",south="); Serial.print(south? "true" : "false");
Serial.print(",west="); Serial.println(west? "true" : "false");
*/
Serial.print(y); Serial.print(","); Serial.print(x);
if(east) Serial.print(",east=true");
if(west) Serial.print(",west=true");
if(south) Serial.print(",south=true");
if(north) Serial.print(",north=true");
Serial.println();
}
Final Integration
#include < Servo.h >
#include "arduinoFFT.h"
#include < SPI.h >
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"
RF24 radio(9, 10);
const uint64_t pipes[2] = { 0x0000000026LL, 0x0000000027LL };
typedef enum { robot = 1, base_station } role_e;
const char* role_friendly_name[] = { "invalid", "Ping out", "Pong back"};
role_e role = robot;
int rows = 2; //change grid size depending on maze
int cols = 3;
byte grid[3][2];
byte orientation = 1; // right = 1, up = 2, left = 4, down = 8
byte right = 1;
byte up = 2;
byte left = 4;
byte down = 8;
byte posX = -1; //to take into account that we will detect corner right when we start
byte posY = rows - 1;
arduinoFFT FFT = arduinoFFT();
#define CHANNEL A0
const uint16_t samples = 128; //This value MUST ALWAYS be a power of 2
const double samplingFrequency = 20000; //Hz, must be less than 10000 due to ADC
unsigned int sampling_period_us;
unsigned long microseconds;
#include < Adafruit_NeoPixel.h >
#ifdef __AVR__
#include < avr/power.h >
#endif
#define PIN 7
Adafruit_NeoPixel strip = Adafruit_NeoPixel(10, PIN, NEO_GRB + NEO_KHZ800);
#define SCL_INDEX 0x00
#define SCL_TIME 0x01
#define SCL_FREQUENCY 0x02
#define SCL_PLOT 0x03
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
double vReal[samples];
double vImag[samples];
int line_sens_pin[3] = {2, 3, 4};
long line_sens_data_time[3] = {0, 0, 0};
bool line_sens_data_digi[3] = {0, 0, 0};
#define STRAIGHT 0
#define RIGHT 1
#define LEFT -1
int threshold = 250;
int counter = 0;
long corner_debounce = -1000;
int turn_delay = 750;
bool front_wall;
bool right_wall;
int front_wall_pin = A3;
int right_wall_pin = A5;
int wall_threshold = 175;
bool enabled = true;
Servo leftServo;
Servo rightServo;
int leftServo_pin = 6;
int rightServo_pin = 5;
long info = 0;
bool mazeUpdated = true;
void setup() {
sampling_period_us = round(1000000 * (1.0 / samplingFrequency));
sbi(ADCSRA, ADPS2) ;
cbi(ADCSRA, ADPS1) ;
cbi(ADCSRA, ADPS0) ;
Serial.begin(57600);
printf_begin();
radio.begin();
radio.setRetries(15, 15);
radio.setAutoAck(true);
radio.setChannel(0x50);
radio.setPALevel(RF24_PA_MIN);
radio.setDataRate(RF24_250KBPS);
radio.openWritingPipe(pipes[0]);
radio.openReadingPipe(1, pipes[1]);
radio.startListening();
radio.printDetails();
//initialize grid
// setup grid boundaries as walls
for (int row = 0; row < rows; row++) {
// 0, row -- west = true
grid[0][row] |= left;
grid[cols - 1][row] |= right;
// cols-1, row -- east = true
}
for (int col = 0; col < cols; col++) {
grid[col][0] |= up;
grid[col][rows - 1] |= down;
// col, 0 -- north = true
// col, rows-1 -- south = true
}
strip.begin();
strip.show();
sample(A0);
int f = compute();
int mag = vReal[(int)(f * samples / samplingFrequency)]; //find the magnitude of dominant frequency
bool ready_to_go = false;
enable_motor();
leftServo.write(90);
rightServo.write(90);
while (!ready_to_go) {
delay(1);
sample(A0);
int f = compute();
int mag = vReal[(int)(f * samples / samplingFrequency)]; //find the magnitude of dominant frequency
for (int i = 0; i < samples; i++) {
double scaler = i / (double)samples * samplingFrequency;
if (vReal[i] > 500 && scaler > 600 && scaler < 700) {
ready_to_go = true;
}
}
}
}
void loop() {
if (enabled) {
for (int i = 0; i < 50; i++) {
read_line();
find_direction();
start();
}
}
else {
stop_motors();
}
sample(A1);
int f = compute();
int mag = vReal[(int)(f * samples / samplingFrequency)]; //find the magnitude of dominant frequency
enabled = !(mag > 500 && f > 6000 && f < 12000);
Serial.print("f ");
Serial.println(f);
Serial.print("mag ");
Serial.println(mag);
}
long readQD(int pin) {
pinMode( pin, OUTPUT );
digitalWrite( pin, HIGH );
delayMicroseconds(10);
pinMode(pin, INPUT);
long time = micros();
while (digitalRead(pin) == HIGH && ((micros() - time) < 3000));
return (micros() - time);
}
void read_line() {
for (int i = 0; i <= 2; i++) {
line_sens_data_time[i] = readQD(line_sens_pin[i]);
line_sens_data_digi[i] = (line_sens_data_time[i] < threshold);
}
}
void print_visualizer() {
for (int y = 0; y <= 3; y++) {
Serial.print(line_sens_data_time[y]);
Serial.print(" ");
}
Serial.println();
}
bool detect_corner() {
return line_sens_data_digi[0] && line_sens_data_digi[1] && line_sens_data_digi[2]; //return center&&(left || right)
}
void start() {
if (detect_corner()) {
if (millis() - corner_debounce > 1000) {
right_wall = analogRead(A5) > 175;
front_wall = analogRead(A3) > 175;
updateMaze(front_wall, right_wall);
corner_debounce = millis();
}
corner_detected();
}
else {
follow_line();
strip.setPixelColor(1, 0x0000FF);
strip.show();
}
}
bool detect_wall(int pin) {
return analogRead(pin) > wall_threshold;
}
int find_direction() {
right_wall = detect_wall(right_wall_pin);
front_wall = detect_wall(front_wall_pin);
delay(2);
if (!right_wall) {
strip.setPixelColor(3, strip.Color(255, 0, 0));
strip.show();
return 1;
}
if (!front_wall && right_wall) {
strip.setPixelColor(3, strip.Color(0, 255, 0));
strip.show();
return 0;
}
if (front_wall && right_wall) {
strip.setPixelColor(3, strip.Color(0, 0, 255));
strip.show();
return -1;
}
}
void follow_line() {
if (!mazeUpdated) {
long beginTime = millis();
transmit(info);
Serial.print("transmitting ");
Serial.println(millis() - beginTime);
mazeUpdated = true;
}
int direct = detect_direction();
if (direct != 0)slight_turn(direct) ;
else move_forward();
}
int detect_direction() {
if (line_sens_data_digi[2]) {
return 1;
}
else if (line_sens_data_digi[0]) {
return -1;
}
else if (line_sens_data_digi[1]) {
return 0;
}
}
void move_forward() {
leftServo.write(180);
rightServo.write(0);
}
void slight_turn(int direct) {
if (direct == RIGHT) {
leftServo.write(180);
rightServo.write(87);
} else {
leftServo.write(95);
rightServo.write(0);
}
}
void turn(int direct) {
updateOrientation(direct);
if (direct == RIGHT) {
leftServo.write(180);
rightServo.write(180);
delay(750);
find_direction();
if (!front_wall) {
leftServo.write(180);
rightServo.write(0);
delay(200);
}
} else {
leftServo.write(0);
rightServo.write(0);
delay(750);
corner_detected();
}
}
void sample(int pin) {
disable_motor();
for (int i = 0; i < samples; i++)
{
microseconds = micros(); //Overflows after around 70 minutes!
vReal[i] = analogRead(pin);
vImag[i] = 0;
while (micros() < (microseconds + sampling_period_us)) {
//empty loop
}
}
enable_motor();
}
int compute() {
FFT.Windowing(vReal, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD); /* Weigh data */
FFT.Compute(vReal, vImag, samples, FFT_FORWARD); /* Compute FFT */
FFT.ComplexToMagnitude(vReal, vImag, samples); /* Compute magnitudes */
int f = FFT.MajorPeak(vReal, samples, samplingFrequency);
return (f);
}
void enable_motor() {
leftServo.attach(leftServo_pin);
rightServo.attach(rightServo_pin);
}
void disable_motor() {
leftServo.detach();
rightServo.detach();
delay(30);
}
void stop_motors() {
leftServo.write(90);
rightServo.write(90);
delay(10);
for (int j = 0; j < 256 * 5; j += 10) { // 5 cycles of all colors on wheel
for (int i = 0; i < strip.numPixels(); i++) {
strip.setPixelColor(i, Wheel(((i * 256 / strip.numPixels()) + j) & 255));
}
delay(10);
strip.show();
}
delay(10);
for (int i = 0; i < strip.numPixels(); i++) {
strip.setPixelColor(i, 0);
}
}
void corner_detected() {
find_direction();
strip.setPixelColor(1, 0xFFFFFF);
strip.show();
int current_dir = find_direction();
if (current_dir == 0)
{
move_forward();
}
else turn(current_dir);
}
uint32_t Wheel(byte WheelPos) {
WheelPos = 255 - WheelPos;
if (WheelPos < 85) {
return strip.Color(255 - WheelPos * 3, 0, WheelPos * 3);
}
if (WheelPos < 170) {
WheelPos -= 85;
return strip.Color(0, WheelPos * 3, 255 - WheelPos * 3);
}
WheelPos -= 170;
return strip.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
}
void updateMaze(bool frontSensor, bool rightSensor) {
if (orientation == right) {
posX += 1;
}
else if (orientation == up) {
posY -= 1;
}
else if (orientation == left) {
posX -= 1;
}
else if (orientation == down) {
posY += 1;
}
//update grid
grid[posX][posY] |= frontSensor * orientation | rightSensor * orientation * 8 | frontSensor * orientation >> 16 | rightSensor * orientation * 8 >> 16;
info = 0;
info |= ((long)posX) << 3 * 8;
info |= ((long)posY) << 2 * 8;
info |= ((long)grid[posX][posY]) << 1 * 8;
mazeUpdated = false;
}
void updateOrientation(int turnDirection) {
if (turnDirection == -1) {
orientation = (orientation * 2) % 15;
}
else if (turnDirection == 1) {
orientation = (orientation * 8) % 15; //equivalent to turning left 3 times
}
}
void transmit(unsigned long data) {
radio.stopListening();
printf("Now sending %lu...", data);
bool ok = radio.write( &data, sizeof(unsigned long) );
if (ok) printf("ok...");
else printf("failed.\n\r");
radio.startListening();
unsigned long started_waiting_at = millis();
bool timeout = false;
if (timeout) printf("Failed, response timed out.\n\r");
else
{
unsigned long got_data;
radio.read( &got_data, sizeof(unsigned long) );
printf("Got response", got_data);
}
}
Lab 4
Arduino Code
#include
#define OV7670_I2C_ADDRESS 0x21
const int analogInPin = A0; // Analog input pin that the potentiometer is attached to
///////// Main Program //////////////
void setup() {
Wire.begin();
Serial.begin(9600);
write_key_registers();
set_color_matrix();
read_key_registers();
}
void loop() {
delay(20);
}
void write_key_registers() {
OV7670_write_register(0x12, 0x80); //Reset all registers
delay(100);
OV7670_write_register(0x11, 0xC0);
OV7670_write_register(0x0C, 0x08);
OV7670_write_register(0x12, 0x0c); //0C -- disable test, 0E -- enable test
OV7670_write_register(0x40, 0xD0); //Changed from 0F to 0D
OV7670_write_register(0x42, 0x00); //00 -- disable test, 08 -- enable test
OV7670_write_register(0x14, 0x50);
//OV7670_write_register(0x01, 0x00); //blue gain
}
void read_key_registers() {
Serial.print("0x11: ");
Serial.println(read_register_value(0x11), HEX);
Serial.print("0x1e: ");
Serial.println(read_register_value(0x1e), HEX);
Serial.print("0x0c: ");
Serial.println(read_register_value(0x0c), HEX);
Serial.print("0x12: ");
Serial.println(read_register_value(0x12), HEX);
Serial.print("0x40: ");
Serial.println(read_register_value(0x40), HEX);
Serial.print("0x42: ");
Serial.println(read_register_value(0x42), HEX);
}
byte read_register_value(int register_address) {
byte data = 0;
Wire.beginTransmission(OV7670_I2C_ADDRESS);
Wire.write(register_address);
Wire.endTransmission();
Wire.requestFrom(OV7670_I2C_ADDRESS, 1);
while (Wire.available() < 1);
data = Wire.read();
return data;
}
String OV7670_write(int start, const byte *pData, int size) {
int n, error;
Wire.beginTransmission(OV7670_I2C_ADDRESS);
n = Wire.write(start);
if (n != 1) {
return "I2C ERROR WRITING START ADDRESS";
}
n = Wire.write(pData, size);
if (n != size) {
return "I2C ERROR WRITING DATA";
}
error = Wire.endTransmission(true);
if (error != 0) {
return String(error);
}
return "no errors :)";
}
String OV7670_write_register(int reg_address, byte data) {
return OV7670_write(reg_address, &data, 1);
}
void set_color_matrix() {
OV7670_write_register(0x4f, 0x80);
OV7670_write_register(0x50, 0x80);
OV7670_write_register(0x51, 0x00);
OV7670_write_register(0x52, 0x22);
OV7670_write_register(0x53, 0x5e);
OV7670_write_register(0x54, 0x80);
OV7670_write_register(0x56, 0x40);
OV7670_write_register(0x58, 0x9e);
OV7670_write_register(0x59, 0x88);
OV7670_write_register(0x5a, 0x88);
OV7670_write_register(0x5b, 0x44);
OV7670_write_register(0x5c, 0x67);
OV7670_write_register(0x5d, 0x49);
OV7670_write_register(0x5e, 0x0e);
OV7670_write_register(0x69, 0x00);
OV7670_write_register(0x6a, 0x40);
OV7670_write_register(0x6b, 0x0a);
OV7670_write_register(0x6c, 0x0a);
OV7670_write_register(0x6d, 0x55);
OV7670_write_register(0x6e, 0x11);
OV7670_write_register(0x6f, 0x9f);
OV7670_write_register(0xb0, 0x84);
}
Verilog Code
`define SCREEN_WIDTH 176
`define SCREEN_HEIGHT 144
///////* DON'T CHANGE THIS PART *///////
module DE0_NANO(
CLOCK_50,
GPIO_0_D,
GPIO_1_D,
KEY
);
//=======================================================
// PARAMETER declarations
//=======================================================
localparam RED = 8'b111_000_00;
localparam GREEN = 8'b000_111_00;
localparam BLUE = 8'b000_000_11;
//=======================================================
// PORT declarations
//=======================================================
//////////// CLOCK - DON'T NEED TO CHANGE THIS //////////
input CLOCK_50;
//////////// GPIO_0, GPIO_0 connect to GPIO Default //////////
output [33:0] GPIO_0_D;
//////////// GPIO_0, GPIO_1 connect to GPIO Default //////////
input [33:0] GPIO_1_D;
input [1:0] KEY;
///// PIXEL DATA /////
reg [7:0] pixel_data_RGB332 = 8'd0;
///// READ/WRITE ADDRESS /////
reg [14:0] X_ADDR;
reg [14:0] Y_ADDR;
wire [14:0] WRITE_ADDRESS;
reg [14:0] READ_ADDRESS;
reg hflag = 0; //horizontal flag
assign WRITE_ADDRESS = X_ADDR + Y_ADDR*(`SCREEN_WIDTH);
//wire C0_to_GPIO_02;
wire PLL_24;
wire PLL_50;
wire PLL_25;
wire PCLK;
wire HREF;
wire VSYNC;
// Camera stuff
assign HREF = GPIO_1_D[29];
assign VSYNC = GPIO_1_D[30];
assign PCLK = GPIO_1_D[32];
reg [7:0] color_temp;
reg [2:0] temp_counter;
///// VGA INPUTS/OUTPUTS /////
wire VGA_RESET;
wire [7:0] VGA_COLOR_IN;
wire [9:0] VGA_PIXEL_X;
wire [9:0] VGA_PIXEL_Y;
wire [7:0] MEM_OUTPUT;
wire VGA_VSYNC_NEG;
wire VGA_HSYNC_NEG;
reg VGA_READ_MEM_EN;
assign GPIO_0_D[5] = VGA_VSYNC_NEG;
assign VGA_RESET = ~KEY[0];
assign GPIO_0_D[33] = PLL_24;
///// Variables for Img Proc /////
reg [14:0] blue_px;
reg [14:0] red_px;
reg[14:0] green_px;
wire [1:0] b_blue_threshold;
wire [2:0] b_red_threshold;
wire [2:0] b_green_threshold;
wire [1:0] r_blue_threshold;
wire [2:0] r_red_threshold;
wire [2:0] r_green_threshold;
wire [14:0] px_threshold;
reg [2:0] shape_n_color;
reg [8:0] RESULT;
reg [7:0] background = 8'b0;
assign b_blue_threshold = 2'b01;
assign b_red_threshold = 3'b010;
assign b_green_threshold = 3'b010;
assign r_blue_threshold = 2'b10;
assign r_red_threshold = 3'b101;
assign r_green_threshold = 3'b100;
assign px_threshold = 15'd750;
// I/O for img processor
assign GPIO_0_D[28] = shape_n_color[2];
assign GPIO_0_D[30] = shape_n_color[1];
assign GPIO_0_D[32] = shape_n_color[0];
/* WRITE ENABLE */
reg W_EN;
///////* CREATE ANY LOCAL WIRES YOU NEED FOR YOUR PLL *///////
///////* INSTANTIATE YOUR PLL HERE *///////
sweetPLL sweetPLL_inst (
.inclk0 ( CLOCK_50 ),
.c0 ( PLL_24 ),
.c1 ( PLL_25 ),
.c2 ( PLL_50 )
);
///////* M9K Module *///////
Dual_Port_RAM_M9K mem(
.input_data(pixel_data_RGB332),
.w_addr(WRITE_ADDRESS),
.r_addr(READ_ADDRESS),
.w_en(W_EN),
.clk_W(PLL_50),
.clk_R(PLL_25), // DO WE NEED TO READ SLOWER THAN WRITE??
.output_data(MEM_OUTPUT)
);
///////* VGA Module *///////
VGA_DRIVER driver (
.RESET(VGA_RESET),
.CLOCK(PLL_25),
.PIXEL_COLOR_IN(VGA_READ_MEM_EN ? MEM_OUTPUT : background),
.PIXEL_X(VGA_PIXEL_X),
.PIXEL_Y(VGA_PIXEL_Y),
.PIXEL_COLOR_OUT({GPIO_0_D[9],GPIO_0_D[11],GPIO_0_D[13],GPIO_0_D[15],GPIO_0_D[17],GPIO_0_D[19],GPIO_0_D[21],GPIO_0_D[23]}),
.H_SYNC_NEG(GPIO_0_D[7]),
.V_SYNC_NEG(VGA_VSYNC_NEG)
);
/////////* Image Processor *///////
//IMAGE_PROCESSOR proc(
// .PIXEL_IN(MEM_OUTPUT),
// .CLK(PLL_25),
// .VGA_PIXEL_X(VGA_PIXEL_X),
// .VGA_PIXEL_Y(VGA_PIXEL_Y),
// .VGA_VSYNC_NEG(VGA_VSYNC_NEG),
// .RESULT(RESULT)
//);
/////* Update Read Address *///////
always @ (VGA_PIXEL_X, VGA_PIXEL_Y) begin
READ_ADDRESS = (VGA_PIXEL_X + VGA_PIXEL_Y*`SCREEN_WIDTH);
if(VGA_PIXEL_X>(`SCREEN_WIDTH-1) || VGA_PIXEL_Y>(`SCREEN_HEIGHT-1))begin
VGA_READ_MEM_EN = 1'b0;
end
else begin
VGA_READ_MEM_EN = 1'b1;
end
end
///* Camera Reading *///////
reg flag = 1'b0;
reg VSYNCprev;
reg HREFprev;
always @ (posedge PCLK) begin
if(VSYNC && !VSYNCprev) begin
Y_ADDR <= 0;
X_ADDR <= 0;
flag <= 0;
W_EN <= 0;
pixel_data_RGB332[7:0] <= 0;
// Image processor
if ((blue_px > red_px) && (blue_px > px_threshold)) begin
background <= BLUE;
shape_n_color <= 3'b101;
end
else if ((blue_px < red_px) && (red_px > px_threshold)) begin
background <= RED;
shape_n_color <= 3'b001;
end
else begin
background <= GREEN;
shape_n_color <= 3'b000;
end
red_px <= 0;
blue_px <= 0;
end
else if (!HREF && HREFprev) begin
X_ADDR <= 0;
Y_ADDR <= Y_ADDR + 1;
flag <= 0;
W_EN <= 0;
pixel_data_RGB332[7:0] <= 0;
end
else begin
Y_ADDR = Y_ADDR;
if (!flag && HREF) begin
flag = 1'b1;
W_EN = 0;
X_ADDR = X_ADDR;
pixel_data_RGB332[1:0] = {GPIO_1_D[12], GPIO_1_D[11]}; //something is wrong with the cycles, blue being output before red/green, but this code works
end
else if (flag && HREF) begin
pixel_data_RGB332[7:5] = {GPIO_1_D[15], GPIO_1_D[14], GPIO_1_D[13]};
pixel_data_RGB332[4:2] = {GPIO_1_D[10], GPIO_1_D[9], GPIO_1_D[8]};
flag = 1'b0;
W_EN = 1;
X_ADDR = X_ADDR + 1'b1;
// Pixel color counters
if ((pixel_data_RGB332[1:0] >= b_blue_threshold) && (pixel_data_RGB332[7:5] < b_red_threshold) && (pixel_data_RGB332[4:2] < b_green_threshold)) begin
blue_px <= blue_px + 1;
end
else if ((pixel_data_RGB332[1:0] < r_blue_threshold) && (pixel_data_RGB332[7:5] >= r_red_threshold) && (pixel_data_RGB332[4:2] < r_green_threshold)) begin
red_px <= red_px + 1;
end
end
else begin
X_ADDR = 0;
end
end
VSYNCprev = VSYNC;
HREFprev = HREF;
end
endmodule
Milestone 1
Line Detection
long readQD(int pin) { //read from IR sensor
pinMode( pin, OUTPUT );
digitalWrite( pin, HIGH );
delayMicroseconds(10);
pinMode(pin, INPUT);
long time = micros();
while (digitalRead(pin) == HIGH && ((micros() - time) < 3000));
return (micros() - time);
}
void read_line() {
for (int i = 0; i < 5; i++) {
line_sens_data_time[i] = readQD(line_sens_pin[i]);
}
}
void conv_to_digi() {
for (int i = 0; i < 5; i++) {
line_sens_data_digi[i] = (line_sens_data_time[i] < threshold);
}
}
Line Following
int detect_forward_direct() {
if (line_sens_data_digi[2]) { //line on right side
return 1;
}
else if (line_sens_data_digi[0]) { //line on left side
return -1;
}
else if (line_sens_data_digi[1]) { //line in middle
return 0;
}
}
void follow_line() {
last_result = detect_forward_direct();
switch (last_result) {
case 0:
move_forward();
break;
case -1:
move_left();
break;
case 1:
move_right();
break;
default: break;
}
}
Corner Detection
bool detect_corner() {
return line_sens_data_digi[0] && line_sens_data_digi[1] && line_sens_data_digi[2];
}
Figure 8
void start() {
if (detect_corner()) {
int current_dir = directions[counter];
if (current_dir == 0)
{
move_forward();
delay(200);
}
else if (current_dir == 1) {
turn_right();
}
else if (current_dir == -1) {
turn_left();
}
counter++;
if (counter == dir_length) counter = 0;
}
else {
follow_line();
}
}
Milestone 2
Maze exploration and collision avoidance
#include < Servo.h >
#include "arduinoFFT.h"
arduinoFFT FFT = arduinoFFT();
#define CHANNEL A0
const uint16_t samples = 128; //This value MUST ALWAYS be a power of 2
const double samplingFrequency = 20000; //Hz, must be less than 10000 due to ADC
unsigned int sampling_period_us;
unsigned long microseconds;
#include < Adafruit_NeoPixel.h >
#ifdef __AVR__
#include < avr/power.h >
#endif
#define PIN 7
Adafruit_NeoPixel strip = Adafruit_NeoPixel(10, PIN, NEO_GRB + NEO_KHZ800);
#define SCL_INDEX 0x00
#define SCL_TIME 0x01
#define SCL_FREQUENCY 0x02
#define SCL_PLOT 0x03
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
double vReal[samples];
double vImag[samples];
int line_sens_pin[3] = {2, 3, 4};
long line_sens_data_time[3] = {0, 0, 0};
bool line_sens_data_digi[3] = {0, 0, 0};
#define STRAIGHT 0
#define RIGHT 1
#define LEFT -1
int threshold = 250;
int counter = 0;
long corner_time = 0;
int turn_delay = 750;
bool front_wall;
bool right_wall;
bool enabled = true;
Servo leftServo;
Servo rightServo;
int leftServo_pin = 6;
int rightServo_pin = 5;
void setup() {
sampling_period_us = round(1000000 * (1.0 / samplingFrequency));
sbi(ADCSRA, ADPS2) ;
cbi(ADCSRA, ADPS1) ;
cbi(ADCSRA, ADPS0) ;
Serial.begin(115200);
enable_motor();
strip.begin();
strip.show();
}
void loop() {
if (enabled) {
for (int i = 0; i < 50; i++) {
read_line();
//print_visualizer();
find_direction();
start();
}
}
else {
stop_motors();
}
sample();
int f = compute();
int mag = vReal[(int)(f * samples / samplingFrequency)]; //find the magnitude of dominant frequency
enabled = !(mag > 14000 && f > 6000 && f < 12000);
Serial.print("f ");
Serial.println(f);
Serial.print("mag ");
Serial.println(mag);
}
long readQD(int pin) {
pinMode( pin, OUTPUT );
digitalWrite( pin, HIGH );
delayMicroseconds(10);
pinMode(pin, INPUT);
long time = micros();
while (digitalRead(pin) == HIGH && ((micros() - time) < 3000));
return (micros() - time);
}
void read_line() {
for (int i = 0; i <= 2; i++) {
line_sens_data_time[i] = readQD(line_sens_pin[i]);
line_sens_data_digi[i] = (line_sens_data_time[i] < threshold);
}
}
void print_visualizer() {
for (int y = 0; y <= 3; y++) {
Serial.print(line_sens_data_time[y]);
Serial.print(" ");
}
Serial.println();
}
bool detect_corner() {
return line_sens_data_digi[0] && line_sens_data_digi[1] && line_sens_data_digi[2]; //return center&&(left || right)
}
void start() {
if (detect_corner()) {
corner_detected();
}
else {
follow_line();
strip.setPixelColor(1, 0x0000FF);
strip.show();
}
}
int find_direction() {
right_wall = analogRead(A5) > 175;
front_wall = analogRead(A3) > 175;
delay(2);
// Serial.println(right_wall);
// Serial.println(front_wall);
if (!right_wall) {
strip.setPixelColor(3, strip.Color(255, 0, 0));
strip.show();
return 1;
}
if (!front_wall && right_wall) {
strip.setPixelColor(3, strip.Color(0, 255, 0));
strip.show();
return 0;
}
if (front_wall && right_wall) {
strip.setPixelColor(3, strip.Color(0, 0, 255));
strip.show();
return -1;
}
}
void follow_line() {
int direct = detect_direction();
if (direct != 0)slight_turn(direct) ;
else move_forward();
}
int detect_direction() {
if (line_sens_data_digi[2]) {
return 1;
}
else if (line_sens_data_digi[0]) {
return -1;
}
else if (line_sens_data_digi[1]) {
return 0;
}
}
void move_forward() {
leftServo.write(180);
rightServo.write(0);
}
void slight_turn(int direct) {
if (direct == RIGHT) {
leftServo.write(180);
rightServo.write(87);
} else {
leftServo.write(95);
rightServo.write(0);
}
}
void turn(int direct) {
if (direct == RIGHT) {
leftServo.write(180);
rightServo.write(180);
delay(750);
find_direction();
if (!front_wall) {
leftServo.write(180);
rightServo.write(0);
delay(200);
}
} else {
leftServo.write(0);
rightServo.write(0);
delay(750);
corner_detected();
}
}
void sample() {
disable_motor();
for (int i = 0; i < samples; i++)
{
microseconds = micros(); //Overflows after around 70 minutes!
vReal[i] = analogRead(CHANNEL);
vImag[i] = 0;
while (micros() < (microseconds + sampling_period_us)) {
//empty loop
}
}
enable_motor();
}
int compute() {
FFT.Windowing(vReal, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD); /* Weigh data */
FFT.Compute(vReal, vImag, samples, FFT_FORWARD); /* Compute FFT */
FFT.ComplexToMagnitude(vReal, vImag, samples); /* Compute magnitudes */
int f = FFT.MajorPeak(vReal, samples, samplingFrequency);
return (f);
}
void enable_motor() {
leftServo.attach(leftServo_pin);
rightServo.attach(rightServo_pin);
}
void disable_motor() {
leftServo.detach();
rightServo.detach();
delay(30);
}
void stop_motors() {
leftServo.write(90);
rightServo.write(90);
delay(10);
for(int j=0; j < 256*5; j+=10) { // 5 cycles of all colors on wheel
for(int i=0; i< strip.numPixels(); i++) {
strip.setPixelColor(i, Wheel(((i * 256 / strip.numPixels()) + j) & 255));
}
delay(10);
strip.show();
}
delay(10);
for(int i=0; i< strip.numPixels(); i++) {
strip.setPixelColor(i, 0);
}
}
void corner_detected() {
find_direction();
strip.setPixelColor(1, 0xFFFFFF);
strip.show();
int current_dir = find_direction();
if (current_dir == 0)
{
move_forward();
}
else turn(current_dir);
}
uint32_t Wheel(byte WheelPos) {
WheelPos = 255 - WheelPos;
if(WheelPos < 85) {
return strip.Color(255 - WheelPos * 3, 0, WheelPos * 3);
}
if(WheelPos < 170) {
WheelPos -= 85;
return strip.Color(0, WheelPos * 3, 255 - WheelPos * 3);
}
WheelPos -= 170;
return strip.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
}
Milestone 3
Maze exploration
#include < Servo.h >
#include "arduinoFFT.h"
//#include < SPI.h >
#include "nRF24L01.h"
#include "RF24.h"
//#include "printf.h" TODO add this back
struct point {
byte x;
byte y;
point() {
x = 0;
y = 0;
}
point(byte px, byte py) {
x = px;
y = py;
}
};
// class vector{
// point points[81];
// int tail = 0;
// int head = 0;
// public:
// vector(){
// head = 0;
// tail = 0;
// }
// void push_back(point new_element){
// points[head] = new_element;
// head++;
// }
// void pop_back(){
// if(head > 0){
// head--;
// }
// }
// void clear_vector(){
// head = 0;
// tail = 0;
// }
// bool empty(){
// if(head == 0)
// return true;
// return false;
// }
// point get_first(){
// int temp = tail;
// if(tail < head){
// tail++;
// }
// if(tail == head){
// tail = 0;
// head = 0;
// }
// return points[temp];
// }
// };
RF24 radio(9, 10);
const uint64_t pipes[2] = { 0x0000000026LL, 0x0000000027LL };
typedef enum { robot = 1, base_station } role_e;
const char* role_friendly_name[] = { "invalid", "Ping out", "Pong back"};
role_e role = robot;
// vector nodes = vector();
point nodes[81] = {};
int head = 0;
int tail = 0;
int rows = 9; //change grid size depending on maze
int cols = 9;
int grid[9][9] = {};
bool explored_nodes[9][9] = {};
bool startFlag = true;
int orientation = 1; // right = 1, up = 2, left = 4, down = 8
int right = 1;
int up = 2;
int left = 4;
int down = 8;
point pos = point(0, rows - 1);
// byte pos.x = -1; //to take into account that we will detect corner right when we start
// byte pos.y = rows - 1;
arduinoFFT FFT = arduinoFFT();
#define CHANNEL A0
const uint16_t samples = 128; //This value MUST ALWAYS be a power of 2
const double samplingFrequency = 20000; //Hz, must be less than 10000 due to ADC
unsigned int sampling_period_us;
unsigned long microseconds;
#include < Adafruit_NeoPixel.h >
#ifdef __AVR__
#include < avr/power.h >
#endif
#define PIN 7
//Adafruit_NeoPixel strip = Adafruit_NeoPixel(10, PIN, NEO_GRB + NEO_KHZ800);
#define SCL_INDEX 0x00
#define SCL_TIME 0x01
#define SCL_FREQUENCY 0x02
#define SCL_PLOT 0x03
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif
double vReal[samples];
double vImag[samples];
int line_sens_pin[3] = {2, 3, 6};
long line_sens_data_time[3] = {0, 0, 0};
bool line_sens_data_digi[3] = {0, 0, 0};
#define STRAIGHT 0
#define RIGHT 1
#define LEFT -1
int threshold = 270;
int counter = 0;
long corner_debounce = -1000;
int turn_delay = 750;
bool front_wall;
bool right_wall;
bool left_wall;
int front_wall_pin = A4;
int right_wall_pin = A3;
int left_wall_pin = A5;
int wall_threshold = 175;
bool enabled = true;
Servo leftServo;
Servo rightServo;
int leftServo_pin = 4;
int rightServo_pin = 5;
long info = 0;
bool mazeUpdated = true;
void setup() {
Serial.begin(57600);
pinMode(7, OUTPUT);
sampling_period_us = round(1000000 * (1.0 / samplingFrequency));
sbi(ADCSRA, ADPS2) ;
cbi(ADCSRA, ADPS1) ;
cbi(ADCSRA, ADPS0) ;
// printf_begin();
radio.begin();
radio.setRetries(15, 15);
radio.setAutoAck(true);
radio.setChannel(0x50);
radio.setPALevel(RF24_PA_MIN);
radio.setDataRate(RF24_250KBPS);
radio.openWritingPipe(pipes[0]);
radio.openReadingPipe(1, pipes[1]);
radio.startListening();
radio.printDetails();
//initialize grid
// setup grid boundaries as walls
for (int row = 0; row < rows; row++) {
// 0, row -- west = true
grid[0][row] |= left;
grid[cols - 1][row] |= right;
// cols-1, row -- east = true
}
for (int col = 0; col < cols; col++) {
grid[col][0] |= up;
grid[col][rows - 1] |= down;
// col, 0 -- north = true
// col, rows-1 -- south = true
}
// strip.begin();
// strip.show();
// sample(A0);
// int f = compute();
// int mag = vReal[(int)(f * samples / samplingFrequency)]; //find the magnitude of dominant frequency
bool ready_to_go = false;
enable_motor();
leftServo.write(90);
rightServo.write(90);
// while (!ready_to_go) {
// delay(1);
// sample(A0);
// int f = compute();
// int mag = vReal[(int)(f * samples / samplingFrequency)]; //find the magnitude of dominant frequency
// for (int i = 0; i < samples; i++) {
// double scaler = i / (double)samples * samplingFrequency;
// if (vReal[i] > 500 && scaler > 600 && scaler < 700) {
// ready_to_go = true;
// }
// }
// }
}
void loop() {
if (enabled) {
for (int i = 0; i < 50; i++) {
read_line();
// print_visualizer();
// right_wall = analogRead(right_wall_pin) > 175;
// front_wall = analogRead(front_wall_pin) > 175;
// left_wall = analogRead(left_wall_pin) > 175;
// Serial.print("right wall "); Serial.println(analogRead(right_wall_pin));
// delay(2);
// Serial.print("front wall "); Serial.println(analogRead(front_wall_pin));
// delay(2);
// Serial.print("left wall "); Serial.println(analogRead(left_wall_pin));
// delay(100);
start();
}
}
else {
stop_motors();
}
// sample(A1);
// int f = compute();
// int mag = vReal[(int)(f * samples / samplingFrequency)]; //find the magnitude of dominant frequency
// enabled = !(mag > 500 && f > 6000 && f < 12000);
// Serial.print("f ");
// Serial.println(f);
// Serial.print("mag ");
// Serial.println(mag);
}
long readQD(int pin) {
pinMode( pin, OUTPUT );
digitalWrite( pin, HIGH );
delayMicroseconds(10);
pinMode(pin, INPUT);
long time = micros();
while (digitalRead(pin) == HIGH && ((micros() - time) < 3000));
return (micros() - time);
}
void read_line() {
for (int i = 0; i <= 2; i++) {
line_sens_data_time[i] = readQD(line_sens_pin[i]);
line_sens_data_digi[i] = (line_sens_data_time[i] < threshold);
}
}
void print_visualizer() {
for (int y = 0; y <= 2; y++) {
Serial.print(line_sens_data_time[y]);
Serial.print(" ");
}
Serial.println();
}
bool detect_corner() {
return line_sens_data_digi[0] && line_sens_data_digi[1] && line_sens_data_digi[2]; //return center&&(left || right)
}
void start() {
if (detect_corner()) {
if (millis() - corner_debounce > 1000) {
delay(350);
right_wall = analogRead(right_wall_pin) > wall_threshold;
front_wall = analogRead(front_wall_pin) > wall_threshold;
left_wall = analogRead(left_wall_pin) > wall_threshold;
if (startFlag) {
updateMaze(front_wall, left_wall, right_wall);
startFlag = false;
}
corner_debounce = millis();
}
corner_detected();
}
else {
follow_line();
// strip.setPixelColor(1, 0x0000FF);
// strip.show();
}
}
bool detect_wall(int pin) {
return analogRead(pin) > wall_threshold;
}
//int find_direction() {
// right_wall = detect_wall(right_wall_pin);
// front_wall = detect_wall(front_wall_pin);
// left_wall = detect_wall(left_wall_pin);
// delay(2);
//
// if (!right_wall) {
// strip.setPixelColor(3, strip.Color(255, 0, 0));
// strip.show();
//
// return 1;
// }
// if (!front_wall && right_wall) {
// strip.setPixelColor(3, strip.Color(0, 255, 0));
// strip.show();
// return 0;
// }
// if (front_wall && right_wall) {
// strip.setPixelColor(3, strip.Color(0, 0, 255));
// strip.show();
//
// return -1;
// }
//}
void follow_line() {
if (!mazeUpdated) {
long beginTime = millis();
transmit(info);
Serial.print("transmitting ");
Serial.println(millis() - beginTime);
mazeUpdated = true;
}
int direct = detect_direction();
if (direct != 0)slight_turn(direct) ;
else move_forward();
}
int detect_direction() {
if (line_sens_data_digi[2]) {
return 1;
}
else if (line_sens_data_digi[0]) {
return -1;
}
else if (line_sens_data_digi[1]) {
return 0;
}
}
void move_forward() {
leftServo.write(180);
rightServo.write(0);
delay(75);
}
void slight_turn(int direct) {
if (direct == RIGHT) {
leftServo.write(180);
rightServo.write(87);
} else {
leftServo.write(95);
rightServo.write(0);
}
}
void turn(int direct) {
// updateOrientation(direct);
//turning for wall following
// if (direct == RIGHT) {
// leftServo.write(180);
// rightServo.write(180);
// delay(700);
// find_direction();
// if (!front_wall) {
// leftServo.write(180);
// rightServo.write(0);
// delay(200);
// }
// } else {
// leftServo.write(0);
// rightServo.write(0);
// delay(700);
// corner_detected();
// }
delay(200);
if (direct == RIGHT) {
leftServo.write(180);
rightServo.write(180);
delay(350);
while (true) {
read_line();
if (line_sens_data_digi[1]) {
break;
}
}
}
else {
leftServo.write(0);
rightServo.write(0);
delay(350);
while (true) {
read_line();
if (line_sens_data_digi[1]) {
break;
}
}
}
}
void sample(int pin) {
disable_motor();
for (int i = 0; i < samples; i++)
{
microseconds = micros(); //Overflows after around 70 minutes!
vReal[i] = analogRead(pin);
vImag[i] = 0;
while (micros() < (microseconds + sampling_period_us)) {
//empty loop
}
}
enable_motor();
}
//int compute() {
// FFT.Windowing(vReal, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD); /* Weigh data */
// FFT.Compute(vReal, vImag, samples, FFT_FORWARD); /* Compute FFT */
// FFT.ComplexToMagnitude(vReal, vImag, samples); /* Compute magnitudes */
// int f = FFT.MajorPeak(vReal, samples, samplingFrequency);
// return (f);
//}
void enable_motor() {
leftServo.attach(leftServo_pin);
rightServo.attach(rightServo_pin);
}
void disable_motor() {
leftServo.detach();
rightServo.detach();
delay(30);
}
void stop_motors() {
leftServo.write(90);
rightServo.write(90);
delay(10);
// for (int j = 0; j < 256 * 5; j += 10) { // 5 cycles of all colors on wheel
// for (int i = 0; i < strip.numPixels(); i++) {
// strip.setPixelColor(i, Wheel(((i * 256 / strip.numPixels()) + j) & 255));
// }
// delay(10);
// strip.show();
// }
// delay(10);
// for (int i = 0; i < strip.numPixels(); i++) {
// strip.setPixelColor(i, 0);
// }
}
void explore(point origin) {
int maxDepth = 1;
while (true) {
bool depthOverflow = false;
if (navigate(origin, maxDepth, depthOverflow)) {
// nodes.get_first();
return;
}
if (depthOverflow && maxDepth < 30) {
// Serial.print("max depth ");
// Serial.println(maxDepth);
maxDepth++;
}
else {
//explored all nodes -- no unexplored nodes reached
break;
}
}
}
void corner_detected() {
Serial.println("corner detected");
if (head > 0) {
Serial.println("going to unexplored node");
follow_node();
move_forward();
}
else {
Serial.println("reached unexplored node");
//reached new node
right_wall = detect_wall(right_wall_pin);
front_wall = detect_wall(front_wall_pin);
left_wall = detect_wall(left_wall_pin);
Serial.print("right_wall "); Serial.println(right_wall);
Serial.print("front_wall "); Serial.println(front_wall);
Serial.print("left_wall "); Serial.println(left_wall);
updateMaze(front_wall, left_wall, right_wall);
Serial.println("exploring");
digitalWrite(7, HIGH);
explore(pos);
digitalWrite(7, LOW);
if (head > 0) {
Serial.println("head greater than 0");
follow_node();
move_forward();
}
else {
Serial.println("infinity loop");
transmit(info);
leftServo.write(90);
rightServo.write(90);
disable_motor();
while (true); //TODO for now, do nothing once all nodes are explored
}
}
}
//uint32_t Wheel(byte WheelPos) {
// WheelPos = 255 - WheelPos;
// if (WheelPos < 85) {
// return strip.Color(255 - WheelPos * 3, 0, WheelPos * 3);
// }
// if (WheelPos < 170) {
// WheelPos -= 85;
// return strip.Color(0, WheelPos * 3, 255 - WheelPos * 3);
// }
// WheelPos -= 170;
// return strip.Color(WheelPos * 3, 255 - WheelPos * 3, 0);
//}
void updateMaze(bool frontSensor, bool leftSensor, bool rightSensor) {
// if (orientation == right) {
// pos.x += 1;
// }
// else if (orientation == up) {
// pos.y -= 1;
// }
// else if (orientation == left) {
// pos.x -= 1;
// }
// else if (orientation == down) {
// pos.y += 1;
// }
//update grid
grid[pos.x][pos.y] |= frontSensor * orientation | leftSensor * orientation * 2 | rightSensor * orientation * 8;
grid[pos.x][pos.y] |= frontSensor * orientation >> 4 | leftSensor * orientation * 2 >> 4 | rightSensor * orientation * 8 >> 4;
explored_nodes[pos.x][pos.y] = true;
info = 0;
info |= ((long)pos.x) << 3 * 8;
info |= ((long)pos.y) << 2 * 8;
info |= ((long)grid[pos.x][pos.y]) << 1 * 8;
mazeUpdated = false;
}
void updateOrientation(int turnDirection) {
if (turnDirection == -1) {
orientation = (orientation * 2) % 15;
}
else if (turnDirection == 1) {
orientation = (orientation * 8) % 15; //equivalent to turning left 3 times
}
}
void transmit(unsigned long data) {
radio.stopListening();
// printf("Now sending %lu...", data);
bool ok = radio.write( &data, sizeof(unsigned long) );
// if (ok) printf("ok...");
// else printf("failed.\n\r");
radio.startListening();
unsigned long started_waiting_at = millis();
bool timeout = false;
// if (timeout) printf("Failed, response timed out.\n\r");
// else
// {
// unsigned long got_data;
// radio.read( &got_data, sizeof(unsigned long) );
// printf("Got response", got_data);
// }
}
void move(point *pos, point *target) { //invaraint, manhattan distance between pos and target is exactly 1
int dx = target->x - pos->x;
int dy = target->y - pos->y;
int rights = 0;
Serial.print("moving from ");
Serial.print(pos->x);
Serial.print(" ");
Serial.println(pos->y);
Serial.print("moving to ");
Serial.print(target->x);
Serial.print(" ");
Serial.println(target->y);
Serial.print("original orientation");
Serial.println(orientation);
pos->x += dx;
pos->y += dy;
if (dx == 1) { //need to go right
while (orientation != right) {
updateOrientation(right);
rights++;
}
}
else if (dx == -1) { //need to go left
while (orientation != left) {
updateOrientation(right);
rights++;
}
}
else if (dy == -1) { //need to go up
while (orientation != up) {
updateOrientation(right);
rights++;
}
}
else if (dy == 1) { //need to go down
while (orientation != down) {
updateOrientation(right);
rights++;
}
}
if (rights == 3) {
turn(LEFT); //rotate left = rotate right 3 times
}
else {
for (int i = 0; i < rights; i++) {
turn(RIGHT);
}
}
}
//need to figure out max depth reached
bool navigate(point origin, int depth, bool& depthOverflow) { //call navigate if
// Serial.println(grid[origin.x][origin.y]);
// Serial.print("checking ");
// Serial.print(origin.x);
// Serial.print(" ");
// Serial.println(origin.y);
for (int i = 0; i < head - 1; i++) {
if (nodes[i].x == origin.x && nodes[i].y == origin.y) {
return false;
}
}
if (origin.x < 0 || origin.x >= cols || origin.y < 0 || origin.y >= rows) {
// Serial.println("out of bound");
return false;
}
if (!explored_nodes[origin.x][origin.y]) {
// Serial.println("new unexplored node");
return true;
//followNodes(); move this to main
}
else if (depth <= 0) {
// Serial.println("depth maxing out");
depthOverflow = true;
return false;
}
// Serial.println(!(grid[origin.x][origin.y] & left));
if (!(grid[origin.x][origin.y] & left)) { // grid[origin.x][origin.y] & left != 0
nodes[head] = point(origin.x - 1, origin.y);
head++;
if (navigate(point(origin.x - 1, origin.y), depth - 1, depthOverflow)) //point(origin.x-1, origin.y)
return true;
else {
head--;
}
}
// Serial.println(!(grid[origin.x][origin.y] & right));
if (!(grid[origin.x][origin.y] & right)) { // grid[origin.x][origin.y] & right != 0
nodes[head] = point(origin.x + 1, origin.y);
head++;
if (navigate(point(origin.x + 1, origin.y), depth - 1, depthOverflow)) //point(origin.x+1, origin.y)
return true;
else {
head--;
}
}
// Serial.println(!(grid[origin.x][origin.y] & up));
if (!(grid[origin.x][origin.y] & up)) { // grid[origin.x][origin.y] & up != 0
nodes[head] = point(origin.x, origin.y - 1);
head++;
if (navigate(point(origin.x, origin.y - 1), depth - 1, depthOverflow)) //point(origin.x, origin.y-1)
return true;
else {
head--;
}
}
// Serial.println(!(grid[origin.x][origin.y] & down));
if (!(grid[origin.x][origin.y] & down)) { // grid[origin.x][origin.y] & down != 0
nodes[head] = point(origin.x, origin.y + 1);
head++;
if (navigate(point(origin.x, origin.y + 1), depth - 1, depthOverflow))
return true;
else {
head--;
}
}
// Serial.println("nothing evaluates to true :'(");
return false;
}
void follow_node() {
// for(int i = 1; i < stack.size(); i++){ //TODO check initial
move(&pos, &nodes[tail]);
tail++;
if (tail == head) {
tail = 0;
head = 0;
}
// }
//explore node (i.e check walls and transmit)
}
Milestone 4
Arduino Code
#include < Wire.h >
#define OV7670_I2C_ADDRESS 0x21
int readings[40];
const int analogInPin = A0; // Analog input pin that the potentiometer is attached to
///////// Main Program //////////////
void setup() {
Wire.begin();
Serial.begin(9600);
write_key_registers();
set_color_matrix();
read_key_registers();
}
void loop() {
int blue = 0;
int red = 0;
int diamond = 0;
int triangle = 0;
int square = 0;
int color = 0;
int shape = 0;
int max_reading = 1000 * 3.3 / 5;
for (int i = 0; i < 40; i++) {
readings[i] = analogRead(analogInPin);
delay(20);
}
for (int i = 0; i < 40; i++) {
if ( readings[i] >= max_reading / 2) {
readings[i] -= max_reading / 2;
blue++;
shape++;
}
else if (readings[i] > 5) {
red++;
shape++;
}
if ( readings[i] >= max_reading / 4 + max_reading / 8) {
diamond++;
}
else if (readings[i] >= max_reading / 4 ) {
square++;
}
else if ( readings[i] >= max_reading / 8) {
triangle++;
}
}
if (shape > 10) {
if (blue > red) Serial.println("blue"); else Serial.println("red");
if (diamond > triangle + square) Serial.println("diamond");
else if (triangle > diamond + square) Serial.println("triangle");
else if (square > diamond + triangle) Serial.println("square");
else Serial.println("Confused");
}
else {
Serial.println("None");
}
//msb color
//X00 nothing
//x01 triangle
//x10 square
//x11 diamond
// Serial.println(reading);
delay(500);
}
void write_key_registers() {
OV7670_write_register(0x12, 0x80); //Reset all registers
delay(100);
OV7670_write_register(0x11, 0xC0);
OV7670_write_register(0x0C, 0x08);
OV7670_write_register(0x12, 0x0c); //0C -- disable test, 0E -- enable test
OV7670_write_register(0x40, 0xD0); //Changed from 0F to 0D
OV7670_write_register(0x42, 0x00); //00 -- disable test, 08 -- enable test
OV7670_write_register(0x14, 0xB0);
OV7670_write_register(0x01, 0x00); //blue gain
}
void read_key_registers() {
Serial.print("0x11: ");
Serial.println(read_register_value(0x11), HEX);
Serial.print("0x1e: ");
Serial.println(read_register_value(0x1e), HEX);
Serial.print("0x0c: ");
Serial.println(read_register_value(0x0c), HEX);
Serial.print("0x12: ");
Serial.println(read_register_value(0x12), HEX);
Serial.print("0x40: ");
Serial.println(read_register_value(0x40), HEX);
Serial.print("0x42: ");
Serial.println(read_register_value(0x42), HEX);
}
byte read_register_value(int register_address) {
byte data = 0;
Wire.beginTransmission(OV7670_I2C_ADDRESS);
Wire.write(register_address);
Wire.endTransmission();
Wire.requestFrom(OV7670_I2C_ADDRESS, 1);
while (Wire.available() < 1);
data = Wire.read();
return data;
}
String OV7670_write(int start, const byte *pData, int size) {
int n, error;
Wire.beginTransmission(OV7670_I2C_ADDRESS);
n = Wire.write(start);
if (n != 1) {
return "I2C ERROR WRITING START ADDRESS";
}
n = Wire.write(pData, size);
if (n != size) {
return "I2C ERROR WRITING DATA";
}
error = Wire.endTransmission(true);
if (error != 0) {
return String(error);
}
return "no errors :)";
}
String OV7670_write_register(int reg_address, byte data) {
return OV7670_write(reg_address, &data, 1);
}
void set_color_matrix() {
OV7670_write_register(0x4f, 0x80);
OV7670_write_register(0x50, 0x80);
OV7670_write_register(0x51, 0x00);
OV7670_write_register(0x52, 0x22);
OV7670_write_register(0x53, 0x5e);
OV7670_write_register(0x54, 0x80);
OV7670_write_register(0x56, 0x40);
OV7670_write_register(0x58, 0x9e);
OV7670_write_register(0x59, 0x88);
OV7670_write_register(0x5a, 0x88);
OV7670_write_register(0x5b, 0x44);
OV7670_write_register(0x5c, 0x67);
OV7670_write_register(0x5d, 0x49);
OV7670_write_register(0x5e, 0x0e);
OV7670_write_register(0x69, 0x00);
OV7670_write_register(0x6a, 0x40);
OV7670_write_register(0x6b, 0x0a);
OV7670_write_register(0x6c, 0x0a);
OV7670_write_register(0x6d, 0x55);
OV7670_write_register(0x6e, 0x11);
OV7670_write_register(0x6f, 0x9f);
OV7670_write_register(0xb0, 0x84);
}
Verilog Code
`define SCREEN_WIDTH 176
`define SCREEN_HEIGHT 144
///////* DON'T CHANGE THIS PART *///////
module DE0_NANO(
CLOCK_50,
GPIO_0_D,
GPIO_1_D,
KEY
);
//=======================================================
// PARAMETER declarations
//=======================================================
localparam RED = 8'b111_000_00;
localparam GREEN = 8'b000_111_00;
localparam BLUE = 8'b000_000_11;
localparam PINK = 8'b111_100_10;
localparam GREY = 8'b100_100_11;
localparam YELLOW = 8'b100_100_00;
//=======================================================
// PORT declarations
//=======================================================
//////////// CLOCK - DON'T NEED TO CHANGE THIS //////////
input CLOCK_50;
//////////// GPIO_0, GPIO_0 connect to GPIO Default //////////
output [33:0] GPIO_0_D;
//////////// GPIO_0, GPIO_1 connect to GPIO Default //////////
input [33:0] GPIO_1_D;
input [1:0] KEY;
///// PIXEL DATA /////
reg [7:0] pixel_data_RGB332 = 8'd0;
///// READ/WRITE ADDRESS /////
reg [14:0] X_ADDR;
reg [14:0] Y_ADDR;
wire [14:0] WRITE_ADDRESS;
reg [14:0] READ_ADDRESS;
reg hflag = 0; //horizontal flag
assign WRITE_ADDRESS = X_ADDR + Y_ADDR*(`SCREEN_WIDTH);
//wire C0_to_GPIO_02;
wire PLL_24;
wire PLL_50;
wire PLL_25;
wire PCLK;
wire HREF;
wire VSYNC;
// Camera stuff
assign HREF = GPIO_1_D[29];
assign VSYNC = GPIO_1_D[30];
assign PCLK = GPIO_1_D[32];
reg [7:0] color_temp;
reg [2:0] temp_counter;
///// VGA INPUTS/OUTPUTS /////
wire VGA_RESET;
wire [7:0] VGA_COLOR_IN;
wire [9:0] VGA_PIXEL_X;
wire [9:0] VGA_PIXEL_Y;
wire [7:0] MEM_OUTPUT;
wire VGA_VSYNC_NEG;
wire VGA_HSYNC_NEG;
reg VGA_READ_MEM_EN;
assign GPIO_0_D[5] = VGA_VSYNC_NEG;
assign VGA_RESET = ~KEY[0];
assign GPIO_0_D[33] = PLL_24;
///// Variables for Img Proc /////
reg [14:0] blue_px;
reg [14:0] red_px;
reg[14:0] green_px;
wire [1:0] b_blue_threshold;
wire [2:0] b_red_threshold;
wire [2:0] b_green_threshold;
wire [1:0] r_blue_threshold;
wire [2:0] r_red_threshold;
wire [2:0] r_green_threshold;
wire [14:0] px_threshold;
reg [2:0] shape_n_color;
reg [8:0] RESULT;
reg [7:0] background = 8'b0;
reg [9:0] firstpix = 10'b0;
reg firstpixFlag = 0;
reg [9:0] lastpix = 10'b0;
reg lastpixFlag = 0;
wire [7:0] startSection;
assign startSection = 8'd20;
wire [7:0] sectionWidth;
assign sectionWidth = 8'd25;
reg [10:0] firstX = 0;
reg [10:0] midX1 = 0;
reg [10:0] midX2 = 0;
reg [10:0] endX = 0;
reg [10:0] counter_firstX = 0;
reg [10:0] counter_midX = 0;
reg [10:0] counter_endX = 0;
assign b_blue_threshold = 2'b01;
assign b_red_threshold = 3'b010;
assign b_green_threshold = 3'b010;
assign r_blue_threshold = 2'b10;
assign r_red_threshold = 3'b101;
assign r_green_threshold = 3'b100;
assign px_threshold = 15'd750;
// I/O for img processor
assign GPIO_0_D[28] = shape_n_color[2];
assign GPIO_0_D[30] = shape_n_color[1];
assign GPIO_0_D[32] = shape_n_color[0];
/* WRITE ENABLE */
reg W_EN;
///////* CREATE ANY LOCAL WIRES YOU NEED FOR YOUR PLL *///////
///////* INSTANTIATE YOUR PLL HERE *///////
sweetPLL sweetPLL_inst (
.inclk0 ( CLOCK_50 ),
.c0 ( PLL_24 ),
.c1 ( PLL_25 ),
.c2 ( PLL_50 )
);
///////* M9K Module *///////
Dual_Port_RAM_M9K mem(
.input_data(pixel_data_RGB332),
.w_addr(WRITE_ADDRESS),
.r_addr(READ_ADDRESS),
.w_en(W_EN),
.clk_W(PLL_50),
.clk_R(PLL_25), // DO WE NEED TO READ SLOWER THAN WRITE??
.output_data(MEM_OUTPUT)
);
///////* VGA Module *///////
VGA_DRIVER driver (
.RESET(VGA_RESET),
.CLOCK(PLL_25),
.PIXEL_COLOR_IN(VGA_READ_MEM_EN ? MEM_OUTPUT : background),
.PIXEL_X(VGA_PIXEL_X),
.PIXEL_Y(VGA_PIXEL_Y),
.PIXEL_COLOR_OUT({GPIO_0_D[9],GPIO_0_D[11],GPIO_0_D[13],GPIO_0_D[15],GPIO_0_D[17],GPIO_0_D[19],GPIO_0_D[21],GPIO_0_D[23]}),
.H_SYNC_NEG(GPIO_0_D[7]),
.V_SYNC_NEG(VGA_VSYNC_NEG)
);
/////////* Image Processor *///////
//IMAGE_PROCESSOR proc(
// .PIXEL_IN(MEM_OUTPUT),
// .CLK(PLL_25),
// .VGA_PIXEL_X(VGA_PIXEL_X),
// .VGA_PIXEL_Y(VGA_PIXEL_Y),
// .VGA_VSYNC_NEG(VGA_VSYNC_NEG),
// .RESULT(RESULT)
//);
/////* Update Read Address *///////
always @ (VGA_PIXEL_X, VGA_PIXEL_Y) begin
READ_ADDRESS = (VGA_PIXEL_X + VGA_PIXEL_Y*`SCREEN_WIDTH);
if(VGA_PIXEL_X>(`SCREEN_WIDTH-1) || VGA_PIXEL_Y>(`SCREEN_HEIGHT-1))begin
VGA_READ_MEM_EN = 1'b0;
end
else begin
VGA_READ_MEM_EN = 1'b1;
end
end
///* Camera Reading *///////
reg flag = 1'b0;
reg VSYNCprev;
reg HREFprev;
always @ (posedge PCLK) begin
if(VSYNC && !VSYNCprev) begin
Y_ADDR <= 0;
X_ADDR <= 0;
flag <= 0;
W_EN <= 0;
pixel_data_RGB332[7:0] <= 0;
firstX <= 0;
midX1 <= 0;
midX2 <= 0;
endX <= 0;
// Image processor
if ((blue_px > red_px) && (blue_px > px_threshold)) begin
if(firstX+midX2 > midX1+endX + 50) begin
background <= BLUE;
shape_n_color <= 3'b101;
end
else if( firstX+endX < midX1+midX2)begin
background <= BLUE;
shape_n_color <= 3'b111;
end
else begin
background <= BLUE;
shape_n_color <= 3'b110;
end
end
else if ((blue_px < red_px) && (red_px > px_threshold)) begin
if(firstX+midX2 > midX1+endX + 40) begin
background <= RED;
shape_n_color <= 3'b001;
end
else if(firstX+endX < midX1+midX2)begin
background <= RED;
shape_n_color <= 3'b011;
end
else begin
background <= RED;
shape_n_color <= 3'b010;
end
end
else begin
background <= GREEN;
shape_n_color <= 3'b000;
end
red_px <= 0;
blue_px <= 0;
end
else if (!HREF && HREFprev) begin
X_ADDR <= 0;
Y_ADDR <= Y_ADDR + 1;
flag <= 0;
W_EN <= 0;
pixel_data_RGB332[7:0] <= 0;
end
else begin
Y_ADDR = Y_ADDR;
if (!flag && HREF) begin
flag = 1'b1;
W_EN = 0;
X_ADDR = X_ADDR;
pixel_data_RGB332[1:0] = {GPIO_1_D[12], GPIO_1_D[11]}; //something is wrong with the cycles, blue being output before red/green, but this code works
end
else if (flag && HREF) begin
pixel_data_RGB332[7:5] = {GPIO_1_D[15], GPIO_1_D[14], GPIO_1_D[13]};
pixel_data_RGB332[4:2] = {GPIO_1_D[10], GPIO_1_D[9], GPIO_1_D[8]};
flag = 1'b0;
W_EN = 1;
X_ADDR <= X_ADDR + 1'b1;
// Pixel color counters
if ((pixel_data_RGB332[1:0] >= b_blue_threshold) && (pixel_data_RGB332[7:5] < b_red_threshold) && (pixel_data_RGB332[4:2] < b_green_threshold)) begin
blue_px = blue_px + 1;
if(X_ADDR > 40 && !firstpixFlag)begin
if( Y_ADDR >= startSection && Y_ADDR < startSection + sectionWidth) begin
firstX <= firstX + X_ADDR;
firstpixFlag <= 1;
end
else if( Y_ADDR >= startSection + sectionWidth && Y_ADDR < startSection + sectionWidth * 2 ) begin
midX1 <= midX1 + X_ADDR;
firstpixFlag <= 1;
end
else if( Y_ADDR >= startSection + 2*sectionWidth && Y_ADDR < startSection + sectionWidth * 3 ) begin
midX2 <= midX2 + X_ADDR;
firstpixFlag <= 1;
end
else if( Y_ADDR >= startSection + 3*sectionWidth && Y_ADDR < startSection + sectionWidth * 4 ) begin
endX <= endX + X_ADDR;
firstpixFlag <= 1;
end
end
end
else if ((pixel_data_RGB332[1:0] < r_blue_threshold) && (pixel_data_RGB332[7:5] >= r_red_threshold) && (pixel_data_RGB332[4:2] < r_green_threshold)) begin
red_px <= red_px + 1;
if(X_ADDR > 20 && !firstpixFlag)begin
if( Y_ADDR >= startSection && Y_ADDR < startSection + sectionWidth) begin
firstX <= firstX + X_ADDR;
firstpixFlag <= 1;
end
else if( Y_ADDR >= startSection + sectionWidth && Y_ADDR < startSection + sectionWidth * 2 ) begin
midX1 <= midX1 + X_ADDR;
firstpixFlag <= 1;
end
else if( Y_ADDR >= startSection + 2*sectionWidth && Y_ADDR < startSection + sectionWidth * 3 ) begin
midX2 <= midX2 + X_ADDR;
firstpixFlag <= 1;
end
else if( Y_ADDR >= startSection + 3*sectionWidth && Y_ADDR < startSection + sectionWidth * 4 ) begin
endX <= endX + X_ADDR;
firstpixFlag <= 1;
end
end
end
end
else begin
X_ADDR = 0;
firstpixFlag <= 0;
end
end
VSYNCprev = VSYNC;
HREFprev = HREF;
end
endmodule
GUI Arduino code