PositionTracking/IRTracking.ino at master · Ladder11/PositionTracking · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
#include <Wire.h>
#include <PVision.h>
#include "KitEncoder.h"
#include "KitMotor.h"
#include "RegulatedMotor.h"
#include "Drivetrain.h"
#include "Servo.h"
#include "FlameSense.h"
#include "ultrasonic.h"
#include <LiquidCrystal.h>
#include "WallFollow.h"

KitEncoder* rightEncoder;
KitEncoder* leftEncoder;
KitMotor* rightMotor;
KitMotor* leftMotor;
RegulatedMotor* leftRegMotor;
RegulatedMotor* rightRegMotor;
Drivetrain* drivetrain;
FlameSense* flameSense;
ultrasonic* frontSensor;
ultrasonic* leftSensor;
ultrasonic* rightSensor;
WallFollow* wallFollow;

LiquidCrystal lcd(40, 41, 42, 43, 44, 45);

// White, Green, Yellow - SPI
// Grey, Purple, White, Black - I2C
// Red 4-Blue, 5-Green, Yellow 6-Blue, 7-Green
// 2nd power - blue

// Yellow - 22, orange - 23

Servo prop;
float distSpeed;

void setup()
{
  Serial.begin(115200);
  Serial.println("Hello");
  lcd.begin(16, 2);

  rightEncoder = new KitEncoder(22);
  leftEncoder = new KitEncoder(23);
  rightMotor = new KitMotor(4, 5);
  leftMotor = new KitMotor(6, 7);

  leftEncoder->initialize();
  rightEncoder->initialize();
  leftMotor->initialize();
  rightMotor->initialize();

  leftRegMotor = new RegulatedMotor(leftMotor, leftEncoder, 50, 64);
  rightRegMotor = new RegulatedMotor(rightMotor, rightEncoder, 50, 64);
  drivetrain = new Drivetrain(leftRegMotor, rightRegMotor, 2.75, 0.3, 5.3125);
  drivetrain->initialize();

  frontSensor = new ultrasonic(26, 27);
  leftSensor = new ultrasonic(28,29);
  rightSensor = new ultrasonic(24, 25);
  wallFollow = new WallFollow(drivetrain, leftSensor, rightSensor);

  flameSense = new FlameSense(40, 30);
  flameSense->initialize();

}

void loop() {
  if (abs(flameSense->flameAngle()*180/3.14) < 10 && frontSensor->distance() < 20) {
    drivetrain->stop();
    prop.attach(11, 1000, 2000);
    prop.write(180);
    delay(40);
    prop.write(0);
    delay(1000);
    prop.write(180);
    while(1){
      if (!flameSense->isFlame()){
        delay(3000);
        if (!flameSense->isFlame()){
          prop.write(0);
          lcd.setCursor(0, 0);
          lcd.print("X: ");
          lcd.print(drivetrain->getXOdoEst(), DEC);
          lcd.setCursor(7, 0);
          lcd.print("Y: ");
          lcd.print(drivetrain->getYOdoEst(), DEC);
          lcd.setCursor(0, 1);
          lcd.print("T: ");
          lcd.print(drivetrain->getOrientOdoEst());
          delay(5000);
        }
      }
    }
  }
  if (flameSense->isFlame()) {
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Flame \n");
    lcd.setCursor(7, 0);
    lcd.print(flameSense->flameAngle()*180/3.1415);
    lcd.setCursor(0, 1);
    lcd.print("Z: ");
    lcd.print(flameSense->flameHeight(frontSensor));
    distSpeed = .25*(frontSensor->distance()-20);
    if (distSpeed > 6) {
      distSpeed = 6;
    }
    drivetrain->drive(distSpeed, flameSense->flameAngle()*180/3.1415*4); //-.01*(800-flameSense->flameDistance())
  } else {
    lcd.clear();
    lcd.setCursor(0,0);
    lcd.print("Nothing\n");
    lcd.setCursor(0,1);
    lcd.print(rightSensor->distance());
    wallFollow->followRightWall(8.0);
  }
}