-
Notifications
You must be signed in to change notification settings - Fork 1
/
BoatController.cpp
102 lines (85 loc) · 2.73 KB
/
BoatController.cpp
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
#include <Arduino.h>
#include "BoatController.h"
//#define DEBUG // enable serial output for debugging
/**********************************************
* BoatController() - Constructor
* Creating a boatcontroller object requires
* passing the constructor a left engine pin
* and a right engine pin number.
**********************************************/
BoatController::BoatController(int leftEngine, int rightEngine)
{
leftEnginePin = leftEngine;
rightEnginePin = rightEngine;
pinMode(leftEnginePin, OUTPUT);
pinMode(rightEnginePin, OUTPUT);
}
/**********************************************
* adjustHeading()
* This function takes a relative bearing, and
* running speed as an argument. Relative
* bearing is the angle in degrees between
* where the boat is heading versus where it's
* supposed to be heading. An angle to the left
* is negative. The angle is measured from the
* target to the front of the boat.
**********************************************/
void BoatController::adjustHeading(double relativeBearing, int speed)
{
double absRelativeBearing = abs(relativeBearing);
int turnSpeed;
/* *** CALCULATE TURNSPEED ***
* In this first section we calculate what the turn speed should be. The further off course, the more one of
* the engines will be slowed to allow a smooth turn */
if (absRelativeBearing < 90)
turnSpeed = speed * (1 - absRelativeBearing / 90);
else
turnSpeed = 0; // Really sharp turn
#ifdef DEBUG
Serial.print("\nRelBearing: ");
Serial.print(relativeBearing);
Serial.print(" ");
#endif
if ( relativeBearing > 0 ) // positive bearing, turn left
{
// Ensures motors won't run while in debug mode
#ifndef DEBUG
analogWrite(leftEnginePin, turnSpeed); // slow down
analogWrite(rightEnginePin, speed); // full speed
#endif
#ifdef DEBUG
Serial.print("Turning left: (");
Serial.print(turnSpeed);
Serial.print(") (");
Serial.print(speed);
Serial.println(")");
#endif
}
else // negative bearing, turn right
{
// Ensures motors won't run while in debug mode
#ifndef DEBUG
analogWrite(leftEnginePin, speed); // full speed
analogWrite(rightEnginePin, turnSpeed); // slow down
#endif
#ifdef DEBUG
Serial.println("Turning right");
Serial.print(speed);
Serial.print(") (");
Serial.print(turnSpeed);
Serial.println(")");
#endif
}
}
/**********************************************
* stopEngines()
* I feel like this one is pretty obvious.
**********************************************/
void BoatController::stopEngines(void)
{
analogWrite(leftEnginePin, 0);
analogWrite(rightEnginePin, 0);
#ifdef DEBUG
Serial.println("Stopping");
#endif
}