-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.ino
134 lines (122 loc) · 3.77 KB
/
main.ino
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
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
// Arduino Pins
#define L293D_LEFT_EN 2
#define L293D_RIGHT_EN 3
#define L293D_LEFT_IN1 5
#define L293D_LEFT_IN2 6
#define L293D_RIGHT_IN1 7
#define L293D_RIGHT_IN2 8
#define TRIG_LEFT 4
#define TRIG_RIGHT 9
#define ECHO_LEFT 10
#define ECHO_RIGHT 11
// Constants
#define HALF_SOUND_SPEED 0.1715
#define TARGET_DISTANCE 15
#define DISTANCE_BETWEEN_ULTRASONIC_SENSORS 12
// Parameters to be tested
#define ANGULAR_SPEED_MULTIPLIER 0.8
#define LEFT_POSITIVE_SPEED_MULTIPLIER 1
#define LEFT_NEGATIVE_SPEED_MULTIPLIER 1.2
#define RIGHT_POSITIVE_SPEED_MULTIPLIER 1
#define RIGHT_NEGATIVE_SPEED_MULTIPLIER 1.2
// 1 for debug, 0 for not
#define DEBUG 1
// Adjust the above parameters
// The below code does not need to be changed
#include "PID.hpp"
PID avgPID(461.9, 517.9, 0.00265, 0.472, 1, 65.37, 1.7, 125, 0.15, 140, 0.2436);
PID angPID(461.9, 517.9, 0.00265, 0.472, 1, 65.37, 1.7, 125, 0.15, 140, 0.2436);
double avgV;
double angV;
#if DEBUG
String* ptr = new String();
#endif
inline double leftIn() {
digitalWrite(TRIG_LEFT, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_LEFT, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_LEFT, LOW);
return pulseIn(ECHO_LEFT, HIGH) * HALF_SOUND_SPEED;
}
inline double rightIn() {
digitalWrite(TRIG_RIGHT, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_RIGHT, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_RIGHT, LOW);
return pulseIn(ECHO_RIGHT, HIGH) * HALF_SOUND_SPEED;
}
void setup()
{
Serial.begin (9600);
pinMode(TRIG_LEFT, OUTPUT);
pinMode(TRIG_RIGHT, OUTPUT);
pinMode(ECHO_LEFT, INPUT);
pinMode(ECHO_RIGHT, INPUT);
pinMode(L293D_LEFT_IN1, OUTPUT);
pinMode(L293D_LEFT_IN2, OUTPUT);
pinMode(L293D_RIGHT_IN1, OUTPUT);
pinMode(L293D_RIGHT_IN2, OUTPUT);
avgV = 0;
angV = 0;
}
void loop() {
double left = leftIn();
double right = rightIn();
double avg = left + right;
double ang = atan2(left - right, DISTANCE_BETWEEN_ULTRASONIC_SENSORS);
#if DEBUG
*ptr = "";
avgV += avgPID.update((avg - TARGET_DISTANCE), millis(), ptr);
Serial.println("Average Speed PID Update:");
Serial.println(*ptr);
*ptr = "";
angV += angPID.update((ang - TARGET_DISTANCE), millis(), ptr) * ANGULAR_SPEED_MULTIPLIER;
Serial.println("Angular Speed Update:");
Serial.println(*ptr);
*ptr = "";
#else
avgV += avgPID.update((avg - TARGET_DISTANCE), millis());
angV += angPID.update((ang - TARGET_DISTANCE), millis()) * ANGULAR_SPEED_MULTIPLIER;
#endif
double leftV = avgV + angV;
double rightV = avgV - angV;
leftOut(leftV);
rightOut(rightV);
delayMicroseconds(10000);
}
inline void leftOut(double leftV) {
#if DEBUG
Serial.println("Left Original Velocity: " + String(leftV));
#endif
leftV *= (leftV < 0)?LEFT_NEGATIVE_SPEED_MULTIPLIER:LEFT_POSITIVE_SPEED_MULTIPLIER;
leftV = CLAMP(leftV, 255, -255);
#if DEBUG
Serial.println("Left Outputted Velocity: " + String(leftV));
#endif
if (leftV > 0) {
analogWrite(L293D_LEFT_IN1, 0);
analogWrite(L293D_LEFT_IN2, leftV);
} else {
analogWrite(L293D_LEFT_IN1, -leftV);
analogWrite(L293D_LEFT_IN2, 0);
}
}
inline void rightOut(double rightV) {
#if DEBUG
Serial.println("Right Original Velocity: " + String(rightV));
#endif
rightV *= (rightV < 0)?RIGHT_NEGATIVE_SPEED_MULTIPLIER:RIGHT_POSITIVE_SPEED_MULTIPLIER;
rightV = CLAMP(rightV, 255, -255);
#if DEBUG
Serial.println("Right Outputted Velocity: " + String(rightV));
#endif
if (rightV > 0) {
analogWrite(L293D_RIGHT_IN1, 0);
analogWrite(L293D_RIGHT_IN2, rightV);
} else {
analogWrite(L293D_RIGHT_IN1, -rightV);
analogWrite(L293D_RIGHT_IN2, 0);
}
}