#include "definitions.h" // contains constants that should be accessible by many files
#include "diff_cntrl.h" // controls the wheels (left and right speed) uses timer0 and timer5 for speed measurement
#include <TimerThree.h> // used for generating timer interrupts for the differential control
#include "A7.h" // used for commmunication using an A7 module through Serial1
#include "M_Ultrasonics.h" // measures distance from 7 ultrasonic sensors using pin change interrupts on A8 - A14 (PORTK) and TimerOne.h
#include "FuzzyBBC.h" // a fuzzy logic behavior-based controller (does only floating point computations
volatile double dists[NUM_US]; // array where the obstacle distances will be stored
volatile uint8_t done = 0; // flag set by M_Ultrasonics.h when all 7 seven distances are measured
FuzzyBBC fbbc; // reference to the behavior based controller
volatile double xG = 0, yG = 0; // coordinates of the goal
volatile double v = 0, w = 0; // desired velocity and angular velocity. set by fbbc and used by the differential controller
// variables holding the current state of the
volatile double x = 0, y = 0, phi = 0; // coordinates and heading
volatile double dx, dy, dphi; // changes in coordinates and heading (used for debugging
// variables used for the speed control of the motors: reference speed, actual speed, counter value, output voltage for left and right motors
volatile double l_ref = 0, l_spd = 0, r_ref = 0, r_spd = 0;
volatile uint8_t l_ctr = 0, r_ctr = 0;
volatile uint8_t l_volt = 0, r_volt = 0;
volatile boolean newvolt = true; // flag set when a new voltage value is computed
volatile boolean updated = false; // flag set to allow transmission of data to the remote machine
volatile uint8_t update_count = 0;
//------------debug variables-----------------
volatile double dvl, dvr;
//--------------------------------------------
void setup()
{
delay(5000);
A7begin(); // starts up the A7 module and connects to a predefined remote machine
initMotors(); // initialize the pins for the motor control (I use an LM293D motor driver)
setup_cntr(); // set the registers for timer0 and timer5
// vw2lr();
fbbc.begin(&xG, &yG, NUM_US, dists, &x, &y, &phi, &v, &w); // starting the fuzzy logic controller, it has direct access to the provided variables
HCRS04.begin(T_US, dists, &done); // starting the ultrasonic sensors controller (it runs every 30 millisecond
Timer3.initialize(T_MOTOR*1000L); // timer interrupts for the speed control (T_MOTOR = 210 milliseconds)
Timer3.attachInterrupt(cntrISR);
}
void loop() {
if(done) // wait for obstacle distances to be updated
{
arbiter(); // update v and w
if(updated && !waiting){ // send data to the remote machine
String info = "v: " + String(v) + "\tw: " + String(w) +
"\t\tvl: " + String(l_spd) + "\tlref: " + String(l_ref) + "\tlvolt: " + String(l_volt) +
"\t\tvr: " + String(r_spd) + "\trref: " + String(r_ref) + "\trvolt: " + String(r_volt) +
"\t\tx: " + String(x) + "\ty: " + String(y) + "\txG: " + String(xG) + "\tyG: " + String(yG);
LogInfo(info);
updated = false;
}
done = false;
}
if (A7_STREAM.available() > 0) { // receive input from the A7 module (Serial1)
String msg = "";
if(waiting){
get_A7_resp(); // read the message (function in A7.h)
msg = resp; // save the message (variable in A7.h)
resp = "";
}
else{
msg = readStr();
}
if (msg.length() > 2) // process the received message
{
if (msg.substring(0, 2).equals("xy")) {
readXYG(msg);
resp = "";
}
else if(msg.equals("stop")){
xG = x;
yG = y;
resp = "";
}
else if(msg.equals("+TCPCLOSE:0")){
disconnected();
}
}
}
}
inline void arbiter(){
fbbc.arbiter();
newvolt = true;
vw2lr(); // convert v and w to left speed and right speed
}
inline void cntrISR() // ISR that runs every 210 milliseconds (for speed control)
{
// compute the speed of the car
read_rm();
read_lm();
ctr2lr(); // convert counter values to centimeter/second
if (newvolt){
spd2volt_init();
newvolt = false;
}
else
spd2volt();
// update motor values
analogWrite(L_PWM_PIN, l_volt);
analogWrite(R_PWM_PIN, r_volt);
updatePos();
}
inline void updatePos() // compute new position (using differential drive kinematic equations)
{
lr2xyphi();
}
inline void readXYG(String cmd) // get the new goal coordinates
{
int sp1 = cmd.indexOf(' ');
int sp2 = cmd.indexOf(' ', sp1 + 1);
xG = (double)cmd.substring(sp1 + 1, sp2).toInt();
yG = (double)cmd.substring(sp2 + 1, cmd.indexOf('~',sp2+1)).toInt();
}
#include "definitions.h" // contains constants that should be accessible by many files
#include "diff_cntrl.h" // controls the wheels (left and right speed) uses timer0 and timer5 for speed measurement
#include <TimerThree.h> // used for generating timer interrupts for the differential control
#include "A7.h" // used for commmunication using an A7 module through Serial1
#include "M_Ultrasonics.h" // measures distance from 7 ultrasonic sensors using pin change interrupts on A8 - A14 (PORTK) and TimerOne.h
#include "FuzzyBBC.h" // a fuzzy logic behavior-based controller (does only floating point computations
volatile double dists[NUM_US]; // array where the obstacle distances will be stored
volatile uint8_t done = 0; // flag set by M_Ultrasonics.h when all 7 seven distances are measured
FuzzyBBC fbbc; // reference to the behavior based controller
volatile double xG = 0, yG = 0; // coordinates of the goal
volatile double v = 0, w = 0; // desired velocity and angular velocity. set by fbbc and used by the differential controller
// variables holding the current state of the
volatile double x = 0, y = 0, phi = 0; // coordinates and heading
volatile double dx, dy, dphi; // changes in coordinates and heading (used for debugging
// variables used for the speed control of the motors: reference speed, actual speed, counter value, output voltage for left and right motors
volatile double l_ref = 0, l_spd = 0, r_ref = 0, r_spd = 0;
volatile uint8_t l_ctr = 0, r_ctr = 0;
volatile uint8_t l_volt = 0, r_volt = 0;
volatile boolean newvolt = true; // flag set when a new voltage value is computed
volatile boolean updated = false; // flag set to allow transmission of data to the remote machine
volatile uint8_t update_count = 0;
//------------debug variables-----------------
volatile double dvl, dvr;
//--------------------------------------------
void setup()
{
delay(5000);
A7begin(); // starts up the A7 module and connects to a predefined remote machine
initMotors(); // initialize the pins for the motor control (I use an LM293D motor driver)
setup_cntr(); // set the registers for timer0 and timer5
// vw2lr();
fbbc.begin(&xG, &yG, NUM_US, dists, &x, &y, &phi, &v, &w); // starting the fuzzy logic controller, it has direct access to the provided variables
HCRS04.begin(T_US, dists, &done); // starting the ultrasonic sensors controller (it runs every 30 millisecond
Timer3.initialize(T_MOTOR*1000L); // timer interrupts for the speed control (T_MOTOR = 210 milliseconds)
Timer3.attachInterrupt(cntrISR);
}
void loop() {
if(done) // wait for obstacle distances to be updated
{
arbiter(); // update v and w
if(updated && !waiting){ // send data to the remote machine
String info = "v: " + String(v) + "\tw: " + String(w) +
"\t\tvl: " + String(l_spd) + "\tlref: " + String(l_ref) + "\tlvolt: " + String(l_volt) +
"\t\tvr: " + String(r_spd) + "\trref: " + String(r_ref) + "\trvolt: " + String(r_volt) +
"\t\tx: " + String(x) + "\ty: " + String(y) + "\txG: " + String(xG) + "\tyG: " + String(yG);
LogInfo(info);
updated = false;
}
done = false;
}
if (A7_STREAM.available() > 0) { // receive input from the A7 module (Serial1)
String msg = "";
if(waiting){
get_A7_resp(); // read the message (function in A7.h)
msg = resp; // save the message (variable in A7.h)
resp = "";
}
else{
msg = readStr();
}
if (msg.length() > 2) // process the received message
{
if (msg.substring(0, 2).equals("xy")) {
readXYG(msg);
resp = "";
}
else if(msg.equals("stop")){
xG = x;
yG = y;
resp = "";
}
else if(msg.equals("+TCPCLOSE:0")){
disconnected();
}
}
}
}
inline void arbiter(){
fbbc.arbiter();
newvolt = true;
vw2lr(); // convert v and w to left speed and right speed
}
inline void cntrISR() // ISR that runs every 210 milliseconds (for speed control)
{
// compute the speed of the car
read_rm();
read_lm();
ctr2lr(); // convert counter values to centimeter/second
if (newvolt){
spd2volt_init();
newvolt = false;
}
else
spd2volt();
// update motor values
analogWrite(L_PWM_PIN, l_volt);
analogWrite(R_PWM_PIN, r_volt);
updatePos();
}
inline void updatePos() // compute new position (using differential drive kinematic equations)
{
lr2xyphi();
}
inline void readXYG(String cmd) // get the new goal coordinates
{
int sp1 = cmd.indexOf(' ');
int sp2 = cmd.indexOf(' ', sp1 + 1);
xG = (double)cmd.substring(sp1 + 1, sp2).toInt();
yG = (double)cmd.substring(sp2 + 1, cmd.indexOf('~',sp2+1)).toInt();
}
inline void setup_cntr()
{
pinMode(CTR0_PIN, INPUT_PULLUP);
pinMode(CTR5_PIN, INPUT_PULLUP);
TCNT5H = 0;
TCNT5L = 0;
TCCR5B = 0X07;
TCNT0 = 0;
TCCR0B = 0X07;
}
inline void initMotors(){
pinMode(L_FWD_PIN, OUTPUT);
pinMode(L_BACK_PIN, OUTPUT);
pinMode(L_PWM_PIN, OUTPUT);
pinMode(L_EN_PIN, OUTPUT);
digitalWrite(L_EN_PIN, HIGH);
digitalWrite(L_FWD_PIN, HIGH);
digitalWrite(L_BACK_PIN, LOW);
pinMode(R_FWD_PIN, OUTPUT);
pinMode(R_BACK_PIN, OUTPUT);
pinMode(R_PWM_PIN, OUTPUT);
pinMode(R_EN_PIN, OUTPUT);
digitalWrite(R_EN_PIN, HIGH);
digitalWrite(R_FWD_PIN, HIGH);
digitalWrite(R_BACK_PIN, LOW);
}
inline void setup_cntr()
{
pinMode(CTR0_PIN, INPUT_PULLUP);
pinMode(CTR5_PIN, INPUT_PULLUP);
TCNT5H = 0;
TCNT5L = 0;
TCCR5B = 0X07;
TCNT0 = 0;
TCCR0B = 0X07;
}
inline void initMotors(){
pinMode(L_FWD_PIN, OUTPUT);
pinMode(L_BACK_PIN, OUTPUT);
pinMode(L_PWM_PIN, OUTPUT);
pinMode(L_EN_PIN, OUTPUT);
digitalWrite(L_EN_PIN, HIGH);
digitalWrite(L_FWD_PIN, HIGH);
digitalWrite(L_BACK_PIN, LOW);
pinMode(R_FWD_PIN, OUTPUT);
pinMode(R_BACK_PIN, OUTPUT);
pinMode(R_PWM_PIN, OUTPUT);
pinMode(R_EN_PIN, OUTPUT);
digitalWrite(R_EN_PIN, HIGH);
digitalWrite(R_FWD_PIN, HIGH);
digitalWrite(R_BACK_PIN, LOW);
}
#include "M_Ultrasonics.h"
M_Ultrasonics HCRS04;
void measISR();
void trigISR();
M_Ultrasonics::M_Ultrasonics()
{
pinIndex = -1;
for(int i=0; i < NUM_US; i++)
{
pinMode(trigs[i],OUTPUT); // trigs: array containing the trigger pins of the ultrasonic sensors
digitalWrite(trigs[i],LOW);
}
}
void M_Ultrasonics::begin(uint32_t period, volatile double *dists_, volatile uint8_t *done_)
{
Timer1.initialize(period*1000);
Timer1.attachInterrupt(trigISR);
dists = dists_;
done = done_;
pinIndex = -1;
for(int i=0; i < NUM_US; i++)
{
pinMode(trigs[i],OUTPUT);
digitalWrite(trigs[i],LOW);
dists[i] = MAX_DIST;
}
enable_pci();
}
void M_Ultrasonics::end()
{
Timer1.stop();
Timer1.detachInterrupt();
PCICR = 0;
}
inline void M_Ultrasonics::trig() // send the trigger signal to the sensor
{
pinIndex ++;
if(pinIndex == NUM_US)
{
pinIndex = 0;
*done = 1;
}
set_pci();
digitalWrite(trigs[pinIndex], HIGH);
delayMicroseconds(ping);
digitalWrite(trigs[pinIndex], LOW);
}
inline void M_Ultrasonics::enable_pci() // enable pin change interrupts on PORTK
{
PCICR = 0x04;
}
inline void M_Ultrasonics::set_pci() // activate interrupts for the current pin
{
PCMSK2 = (1<<pinIndex);
}
inline unsigned long M_Ultrasonics::read_time() // convert the timer value to microseconds
{
return (/*(TCNT1H<<8) | */TCNT1L) * F_CLOCK;
}
inline void M_Ultrasonics::meas()
{
if(PINK & (1<<pinIndex)) { // check if the pin is high
s_echo = /*micros();/*/read_time();
dists[pinIndex] = MAX_DIST*DOBST_COEFF;
}
else
{
f_echo =/* micros();/*/read_time();
// d = (f_echo-s_echo)*soundSpeed/2E4;
if(d < MAX_DIST) {
dists[pinIndex] = d/**DOBST_COEFF*/;
}
// dists[pinIndex] = d; // (double)(f_echo - s_echo);
}
}
ISR (PCINT2_vect) // handle pin change interrupt for A8 to A14 here
{
measISR();
}
void measISR() // ISR called on rising or falling edge on the current pin
{
HCRS04.meas();
}
void trigISR() // function called every 30 millisecond to trigger the next ultrasonic sensor
{
HCRS04.trig();
}
#include "M_Ultrasonics.h"
M_Ultrasonics HCRS04;
void measISR();
void trigISR();
M_Ultrasonics::M_Ultrasonics()
{
pinIndex = -1;
for(int i=0; i < NUM_US; i++)
{
pinMode(trigs[i],OUTPUT); // trigs: array containing the trigger pins of the ultrasonic sensors
digitalWrite(trigs[i],LOW);
}
}
void M_Ultrasonics::begin(uint32_t period, volatile double *dists_, volatile uint8_t *done_)
{
Timer1.initialize(period*1000);
Timer1.attachInterrupt(trigISR);
dists = dists_;
done = done_;
pinIndex = -1;
for(int i=0; i < NUM_US; i++)
{
pinMode(trigs[i],OUTPUT);
digitalWrite(trigs[i],LOW);
dists[i] = MAX_DIST;
}
enable_pci();
}
void M_Ultrasonics::end()
{
Timer1.stop();
Timer1.detachInterrupt();
PCICR = 0;
}
inline void M_Ultrasonics::trig() // send the trigger signal to the sensor
{
pinIndex ++;
if(pinIndex == NUM_US)
{
pinIndex = 0;
*done = 1;
}
set_pci();
digitalWrite(trigs[pinIndex], HIGH);
delayMicroseconds(ping);
digitalWrite(trigs[pinIndex], LOW);
}
inline void M_Ultrasonics::enable_pci() // enable pin change interrupts on PORTK
{
PCICR = 0x04;
}
inline void M_Ultrasonics::set_pci() // activate interrupts for the current pin
{
PCMSK2 = (1<<pinIndex);
}
inline unsigned long M_Ultrasonics::read_time() // convert the timer value to microseconds
{
return (/*(TCNT1H<<8) | */TCNT1L) * F_CLOCK;
}
inline void M_Ultrasonics::meas()
{
if(PINK & (1<<pinIndex)) { // check if the pin is high
s_echo = /*micros();/*/read_time();
dists[pinIndex] = MAX_DIST*DOBST_COEFF;
}
else
{
f_echo =/* micros();/*/read_time();
// d = (f_echo-s_echo)*soundSpeed/2E4;
if(d < MAX_DIST) {
dists[pinIndex] = d/**DOBST_COEFF*/;
}
// dists[pinIndex] = d; // (double)(f_echo - s_echo);
}
}
ISR (PCINT2_vect) // handle pin change interrupt for A8 to A14 here
{
measISR();
}
void measISR() // ISR called on rising or falling edge on the current pin
{
HCRS04.meas();
}
void trigISR() // function called every 30 millisecond to trigger the next ultrasonic sensor
{
HCRS04.trig();
}
