Documente Academic
Documente Profesional
Documente Cultură
//
ALMOST AT COMPETITION
// --------------------------------------------------------------------------#include <math.h>
#include <NewPing.h>
#include <Servo.h> // Servo library disables 9 and 10
//ULTRASOUND
#define TRIGGER_PIN 12 // Arduino pin tied to trigger pin on the ultrasonic se
nsor.
#define ECHO_PIN
11 // Arduino pin tied to echo pin on the ultrasonic senso
r.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters
). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and
maximum distance.
// servo definitions
Servo myservo; // create servo object to control a servo
//servo angles for dumping, 0 is start position, 90 is dump position
int val1 = 0;
// variable to read the value from the analog pin
int val2 = 90;
//initialize servo state
int k = 1;
// Motor Setup with H-bridge
int L = 1;
int motorstate = L; // motor state initialization to spin left when scanning,
may change to optimize
int PrevIRsensor = 0;
int IRsensorValue = 5;
// Left Motor
const int dirPinL = 7; // Digital output pin that the "D" channel is attached t
o (controls motor Direction)
const int enablePinL = 6;
// Right Motor
const int dirPinR= 4; // Digital output pin that the "D" channel is attached to
(controls motor Direction)
const int enablePinR = 5;
int outputValue = 255; // Motor Speed 0 - 255 for both motors
int straight = 1;
float turn = 200.0;
float turn1 = 160.0;
int R = 2;
volatile
volatile
volatile
volatile
volatile
volatile
volatile
volatile
volatile
volatile
//making
int timep1 = 0;
int timep2 = 0;
unsigned long timer1rise1 =0;
unsigned long timer1rise2 =0;
int prev_time1=0;
int risecount1=0;
int timecycle1=0;
int prev_time2=0;
int risecount2=0;
int timecycle2=0;
arrays to store reading values for averaging
INITIALIZATIONS
int lastEncoded1 =
int lastEncoded2 =
long encoderValue1
long encoderValue2
0;
0;
= 0;
= 0;
void setup() {
//Serial.begin(115200); // Open serial monitor at 115200 baud to see ping resu
lts.
//Serial.print("Goal: ");
//Serial.println(goal);
//Serial.print("\t");
//Serial.println(turn);
// Serial.println("Distance: ");
// Serial.println(ultrasound ());
}
else{
analogWrite(enablePinL, 0);
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinR, 0);
digitalWrite(dirPinR,LOW);
}
//Serial.println(timer);
}
int ultrasound () {
delay(50);
// Wait 50ms between pings (about 20 pings/sec
). 29ms should be the shortest delay between pings.
unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (u
S).
//Serial.print("Ping: ");
//Serial.print(uS / US_ROUNDTRIP_CM); // Convert ping time to distance and pri
nt result (0 = outside set distance range, no ping echo)
//Serial.println("cm");
if (uS / US_ROUNDTRIP_CM < 8 && uS/ US_ROUNDTRIP_CM > 0){ // dump chips when d
istance at 8 cm
k = 0;
}
else {
k = 1;
}
//Serial.print("\t");
//Serial.println(k);
return (uS / US_ROUNDTRIP_CM);
}
int TakinDumps(){
// if (k == 0)
// {
analogWrite(enablePinL, 0);
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinR, 0);
digitalWrite(dirPinR,LOW);
myservo.write(val1);
// sets the servo position according t
o the scaled value
delay(1000);// waits for the servo to get there
myservo.write(val2);
//BACK UP AFTER DUMPING
analogWrite(enablePinL, 200);
digitalWrite(dirPinL,LOW);
analogWrite(enablePinR, 200);
digitalWrite(dirPinR,HIGH);
delay(1200);
k = 1;
goal = 4;
//}
//return(k);
return(goal);
}
//Locate 1 kHZ signals
int Locate1kHZ(){ //This function is used to find the 1 Khz signal
//first ir
static int sprev1 = 2; // initialization for reading previous strength
static int s1 = 0;
sprev1 = s1;
static int F1 = 100;
static int Fprev1 = 100;
static int turnfor1=100;
Fprev1 = F1;
F1 = abs(FreqFinal1(Sight1(),averagepulselength1(), FreqID1()));
s1 = abs(Sight1());
//second ir
static int sprev2 = 2; // initialization for reading previous strength
static int s2 = 0;
sprev2 = s2;
static int F2 = 100;
static int Fprev2 = 100;
static int turnfor2=100;
Fprev2 = F2;
F2 = abs(FreqFinal2(Sight2(),averagepulselength2(), FreqID2()));
s2 = abs(Sight2());
// switch states of direction
if(((sprev1 - s1 > 0)||(sprev2-s2>0)) && (motorstate == R || motorstate == 0 )
&& (Fprev1 == 1||Fprev2 ==1)) {
motorstate = L;}
else if(((sprev1 - s1 > 0)||(sprev2-s2>0)) && (motorstate == L || motorstate ==
0 ) && (Fprev1 == 1||Fprev2 ==1)){
motorstate = R;
}
if((int)(1.1-(Sight1()+Sight2())/2)*turn<150){
turnfor1=150;
}
else if((int)(1-(Sight1()+Sight2())/2)*turn>200){
turnfor1=200;
}
else if((int)(1-(Sight1()+Sight2())/2)*turn>150 && ((int)(1-(Sight1()+Sight2())
/2)*turn<200)){
turnfor1=((int)(1-(Sight1()+Sight2())/2)*turn1);
}
if((F1 != 1 && F2 != 1) && motorstate == R) { //no sight, try to find sight by
turning right
analogWrite(enablePinL,turn);
digitalWrite(dirPinL,LOW);
analogWrite(enablePinR,turn);
digitalWrite(dirPinR,LOW);
}
else if((F1 != 1 && F2 != 1) && motorstate == L){ //no sight, try to find sigh
t by turning right
analogWrite(enablePinL,turn);
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinR,turn);
digitalWrite(dirPinR,HIGH);
}
else if ((s1 < 1 || s2 <1) && (F2 == 1 || F1 ==1) && motorstate == R){
analogWrite(enablePinR, (turnfor1));
digitalWrite(dirPinL,LOW);
analogWrite(enablePinL, (turnfor1));
digitalWrite(dirPinR,LOW);
}
else if ((s1 < 1 || s2<1) && (F2 == 1 || F1 == 1) && motorstate == L){
analogWrite(enablePinR, (turnfor1));
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinL, (turnfor1));
digitalWrite(dirPinR,HIGH);
}
else if (F1== 1 || F2==1){ //sight achieved, now pinpoint by strength
analogWrite(enablePinL, 0);
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinR, 0);
digitalWrite(dirPinR,LOW);
goal = 2;
//motorstate = 0; // maybe also return motorstate?
return (goal);
//Now begin to move towards the signal source with a new function
}
}
//Drive towards 1Khz signal
int ATTACK(){ // move towards the signal until a threshold reached!
static int s;
static int F;
static int counter = 0;
static int x = 100; // intialize distance greater than 8
static int z = 0;
z = digitalRead(BUMP);
if (counter < 100){
x =0;
counter ++;}
else if (counter >= 100){
x = ultrasound();
//Serial.print("\t");
//Serial.print("Distance: ");
// Serial.println(x);
counter = 0;
}
//Serial.println(counter);
s = abs(Sight1());
F = abs(FreqFinal1(Sight1(),averagepulselength1(), FreqID1()));
// int x = ultrasound();
if (x > 10 && F == 1 && goal == 2){//if the IR hasn't hit the rails yet, cont
inue moving forward (straight)
analogWrite(enablePinL, outputValue*straight);
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinR, outputValue*straight);
digitalWrite(dirPinR,LOW);
}
else if ( F != 1 ){ // if it has hit the rails, stop - may need to add code to
find the beacon again if it's lost, aka switch the goal state
//analogWrite(enablePinL, 0);
//digitalWrite(dirPinL,HIGH);
//analogWrite(enablePinR, 0);
//digitalWrite(dirPinR,LOW);
//Locate1kHZ();
goal = 1;
}
if( z == HIGH && F == 1){
analogWrite(enablePinL, 0);
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinR, 0);
digitalWrite(dirPinR,LOW);
goal = 3;
}
if (x<= 10 && x > 0 && F == 1 && goal == 2){ // && add encoder distance requir
ement here
analogWrite(enablePinL, 0);
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinR, 0);
digitalWrite(dirPinR,LOW);
int uc1 = ultrasound();
int uc2 = ultrasound();
if ( (uc1 + uc2 + x)/3 <10 && F ==1 && goal ==2){
goal = 3;
}
}
// if (x == 0 && F == 1 && goal == 2){
//
int uc5 = ultrasound();
//
int uc6 = ultrasound();
//
if ( (uc5 + uc6 + x)/3 == 0 && F ==1 && goal ==2){
//
goal = 3;
//
}
// }
return(goal);
}
int Locate5kHZ(){ // essentially, this will be the same code as motorfollow IR,
but for 1 hz signal
static int sprev1 = 2; // initialization for reading previous strength
static int s1 = 0;
sprev1 = s1;
static int F1 = 100;
static int Fprev1 = 100;
static int turnfor1=100;
Fprev1 = F1;
F1 = abs(FreqFinal1(Sight1(),averagepulselength1(), FreqID1()));
s1 = abs(Sight1());
//second ir
static int sprev2 = 2; // initialization for reading previous strength
static int s2 = 0;
sprev2 = s2;
static int F2 = 100;
static int Fprev2 = 100;
static int turnfor5=100;
Fprev2 = F2;
F2 = abs(FreqFinal2(Sight2(),averagepulselength2(), FreqID2()));
s2 = abs(Sight2());
// switch states of direction
return(goal);
}
int Reload(){ // move towards the signal until a threshold reached!
//need to test the threshold
static int s;
static int F;
static int counter1 = 0;
static int y = 100; // intialize distance greater than 8
static int zb = 0;
zb = digitalRead(BUMP2);
if (counter1 < 100){
y = 0;
counter1 ++;}
else if (counter1 >= 100){
y = ultrasound();
//Serial.print("\t");
//Serial.print("Distance: ");
//Serial.println(x);
counter1 = 0;
//Serial.yprint("Distance: ");
//Serial.println(y);
}
s = abs(Sight1());
F = abs(FreqFinal1(Sight1(),averagepulselength1(), FreqID1()));
if (y > 15 && F == 5 && goal == 5){//if the IR hasn't hit the rails yet, contin
ue moving forward (straight)
analogWrite(enablePinL, outputValue*straight);
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinR, outputValue*straight);
digitalWrite(dirPinR,LOW);
}
else if (F != 5) { // if it has hit the rails, stop - may need to add code to
find the beacon again if it's lost, aka switch the goal state
//analogWrite(enablePinL, 0);
//digitalWrite(dirPinL,HIGH);
//analogWrite(enablePinR, 0);
//digitalWrite(dirPinR,LOW);
//Locate5kHZ();
goal = 4 ;
}
if ( y < 20 && y > 0 && F == 5 && goal == 5){ // && add encoder distance requi
rement here
analogWrite(enablePinL, 0);
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinR, 0);
digitalWrite(dirPinR,LOW);
int uc3 = ultrasound();
int uc4 = ultrasound();
if ( (uc3 + uc4 + y)/3 <20 && uc3 > 0 && uc4 > 0 && F ==5 && goal ==5){
goal = 6;
delay(1000);
analogWrite(enablePinL, 200);
digitalWrite(dirPinL,LOW);
analogWrite(enablePinR, 200);
digitalWrite(dirPinR,HIGH);
delay(1000);
}
}
analogWrite(enablePinR, turn1);
digitalWrite(dirPinR,HIGH);
}
}
// Serial.println(x);
return(goal);
}
//code to reset position, hopefully without changing the goals
void Reset(){
static int insight = 0; //check the stuff
static int F1A = 0;
static int F2A = 0;
static int F1B = 0;
static int F2B = 0;
static int timerReset = 0;
if (timerReset == 0){
timerReset++;
F1A = encoderValue1;
F2A = encoderValue2;
//digitalWrite(LED,LOW);
}
if (timerReset <= 1000){
F1B = encoderValue1;
F2B = encoderValue2;
timerReset++;
}
else if (timerReset > 1000 ){
if(F1A==F1B || F2A==F2B ){
analogWrite(enablePinL, outputValue-50);
digitalWrite(dirPinL,LOW);
analogWrite(enablePinR, outputValue-10);
digitalWrite(dirPinR,HIGH);
//digitalWrite(LED,HIGH);
delay(1000);
analogWrite(enablePinL, outputValue);
digitalWrite(dirPinL,HIGH);
analogWrite(enablePinR, outputValue);
digitalWrite(dirPinR,LOW);
delay(250);
}
timerReset = 0;
//LastencoderValue1 =0;
//LastencoderValue2 =0;
}
}
////IR CODE////////////
int FreqFinal1(float S, int Str, float F){
int a=0;
if (S>.3) {
if(4<F && F<6){
a=5;
}
else if(.5<F && F<1.2){
a=1;
}
}
return(a);
}
int FreqFinal2(float S, int Str, float F){
int a=0;
if (S>.3) {
if(4<F && F<6){
a=5;
}
else if(.5<F && F<1.2){
a=1;
}
}
return(a);
}
int averagepulselength1(){
//this just averages the "strength" over the past 10 intterupt samples
totalstrength1=totalstrength1-strengthreadings1[strengthreadIndex1];
strengthreadings1[strengthreadIndex1]=timep1;
totalstrength1=totalstrength1+strengthreadings1[strengthreadIndex1];
strengthreadIndex1=strengthreadIndex1+1;
// if we're at the end of the array...
if (strengthreadIndex1 >= StrengthnumReadings1) {
// ...wrap around to the beginning:
strengthreadIndex1 = 0;
}
// calculate the average:
int average1 = totalstrength1 / StrengthnumReadings1;
return (average1);
}
float FreqID1(){
totalfreq1=totalfreq1-freqreadings1[freqreadIndex1];
freqreadings1[freqreadIndex1]=timecycle1;
totalfreq1=totalfreq1+freqreadings1[freqreadIndex1];
freqreadIndex1=freqreadIndex1+1;
// if we're at the end of the array...
if (freqreadIndex1 >= FreqnumReadings1) {
// ...wrap around to the beginning:
freqreadIndex1 = 0;
}
float FREQaverage1=(1/((float).001*totalfreq1/(float)FreqnumReadings1));
return(FREQaverage1);
}
float Sight1(){
unsigned long timer1=millis();
int sight=0;
if ((timer1-timer1rise1)>20){
sight=0;
}
else{
sight=1;
}
totalsight1=totalsight1-sightreadings1[sightreadIndex1];
sightreadings1[sightreadIndex1]=sight;
totalsight1=totalsight1+sightreadings1[sightreadIndex1];
sightreadIndex1=sightreadIndex1+1;
// if we're at the end of the array...
if (sightreadIndex1 >= SightnumReadings1) {
// ...wrap around to the beginning:
sightreadIndex1 = 0;
}
float Sightaverage1=((float)totalsight1/(float)SightnumReadings1);
return(Sightaverage1);
}
///SECOND IR FUNCTION, ATTACHED TO PIN 3
int averagepulselength2(){
//this just averages the "strength" over the past 10 intterupt samples
totalstrength2=totalstrength2-strengthreadings2[strengthreadIndex2];
strengthreadings2[strengthreadIndex2]=timep2;
totalstrength2=totalstrength2+strengthreadings2[strengthreadIndex2];
strengthreadIndex2=strengthreadIndex2+1;
// if we're at the end of the array...
if (strengthreadIndex2 >= StrengthnumReadings2) {
// ...wrap around to the beginning:
strengthreadIndex2 = 0;
}
// calculate the average:
int average2 = totalstrength2 / StrengthnumReadings2;
return (average2);
}
float FreqID2(){
totalfreq2=totalfreq2-freqreadings2[freqreadIndex2];
freqreadings2[freqreadIndex2]=timecycle2;
totalfreq2=totalfreq2+freqreadings2[freqreadIndex2];
freqreadIndex2=freqreadIndex2+1;
// if we're at the end of the array...
if (freqreadIndex2 >= FreqnumReadings2) {
// ...wrap around to the beginning:
freqreadIndex2 = 0;
}
float FREQaverage2=(1/((float).001*totalfreq2/(float)FreqnumReadings2));
return(FREQaverage2);
}
float Sight2(){
unsigned long timer1=millis();
int sight=0;
if ((timer1-timer1rise2)>20){
sight=0;
}
else{
sight=1;
}
totalsight2=totalsight2-sightreadings2[sightreadIndex2];
sightreadings2[sightreadIndex2]=sight;
totalsight2=totalsight2+sightreadings2[sightreadIndex2];
sightreadIndex2=sightreadIndex2+1;
// if we're at the end of the array...
if (sightreadIndex2 >= SightnumReadings2) {
// ...wrap around to the beginning:
sightreadIndex2 = 0;
}
float Sightaverage2=((float)totalsight2/(float)SightnumReadings2);
return(Sightaverage2);
}
//interrupts(almost too much going on here already, should not get any bigger.
//IR attached to pin 3 INT
void rising2() {
if (risecount2==2){
timecycle2=micros()-prev_time2;
risecount2=0;
}
prev_time2=micros();
attachInterrupt(1, falling2, FALLING);
risecount2=risecount2+1;
timer1rise2=millis();
}
void falling2() {
timep2=abs(micros()-prev_time2);
attachInterrupt(1, rising2, RISING);
}
//IR ATTACHED TO PIN 2 INT
void rising1() {
if (risecount1==2){
timecycle1=micros()-prev_time1;
risecount1=0;
}
prev_time1=micros();
attachInterrupt(0, falling1, FALLING);
risecount1=risecount1+1;
timer1rise1=millis();
}
void falling1() {
timep1=abs(micros()-prev_time1);
attachInterrupt(0, rising1, RISING);
}
//ENCODER CODE
void InitialiseIO(){
pinMode(A0, INPUT);
// Pin A0 is input to which a switch is connected
digitalWrite(A0, HIGH); // Configure internal pull-up resistor
pinMode(A1, OUTPUT);
// Pin A1 is input to which a switch is connected