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
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
| #include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <TFT.h>
#include <SPI.h>
#define OLED_RESET 4
TFT TFTscreen = TFT(6, 7, 8);
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
//Pin Definitions
#define pot0 A0
#define pot1 A1
#define pot2 A2
#define pot3 A3
#define upButtonPin 9
#define downButtonPin 5
// Color definitions
#define BLACK 0x0000
#define YELLOW 0xFFE0
#define WHITE 0xFFFF
uint8_t spacing = 8;
uint8_t yPos = 2;
uint8_t servoNum = 0;
char servo[] = "Servo ";
char colon[] = ": ";
uint8_t upButtonState = 0;
uint8_t downButtonState = 0;
uint8_t activeServoSet = 0;
uint16_t servoPulse[] = {350, 350, 350, 350,
350, 350, 350, 350,
350, 350, 350, 350,
350, 350, 350, 350
};
void setup() {
Serial.begin(9600);
delay(200);
TFTscreen.initR();
pwm.begin();
pwm.setPWMFreq(60);
pinMode(pot0, INPUT);
pinMode(pot1, INPUT);
pinMode(pot2, INPUT);
pinMode(pot3, INPUT);
//Set background colour
TFTscreen.fillScreen(BLACK);
//Write servo numbers
for (uint8_t count = 0; count <= 3; count ++) {
for (uint8_t i = 0; i <= 3; i ++) {
char numRead[2];
char combined[32] = {0};
dtostrf(servoNum, 1, 0, numRead);
strcat(combined, servo);
strcat(combined, numRead);
TFTscreen.drawString(0, yPos, combined, WHITE);
TFTscreen.drawString(48, yPos, colon, WHITE);
servoNum ++;
yPos += spacing;
}
yPos += 8;
}
//Write initial servo positions (350 to start with)
servoNum = 0;
yPos = 2;
for (uint8_t count = 0; count <= 3; count ++) {
for (uint8_t i = 0; i <= 3; i ++) {
char numRead[3];
dtostrf(servoPulse[servoNum], 3, 0, numRead);
TFTscreen.drawString(60, yPos, numRead, YELLOW);
servoNum ++;
yPos += spacing;
}
yPos += 8;
}
TFTscreen.drawString(95, 3, "<", WHITE, 4);
}
void loop() {
//Run function to see if buttons have been pressed, and pick a servo set accordingly
servoSetSelect();
//Record the positions of all servos mapped to a pulsewidth of between 0 and 800
servoPulse[activeServoSet * 4] = map(analogRead(pot0), 0, 1023, 0, 800);
servoPulse[(activeServoSet * 4) + 1] = map(analogRead(pot1), 0, 1023, 0, 800);
servoPulse[(activeServoSet * 4) + 2] = map(analogRead(pot2), 0, 1023, 0, 800);
servoPulse[(activeServoSet * 4) + 3] = map(analogRead(pot3), 0, 1023, 0, 800);
//Clear the previous number, and write the new pulsewidths for the active servo set to the monitor
writePulses ();
//Using the servo driver board, set the active servos to the position specified by the potentiometers
pwm.setPWM(activeServoSet * 4, 0, servoPulse[activeServoSet * 4]);
pwm.setPWM((activeServoSet * 4) + 1, 0, servoPulse[(activeServoSet * 4) + 1]);
pwm.setPWM((activeServoSet * 4) + 2, 0, servoPulse[(activeServoSet * 4) + 2]);
pwm.setPWM((activeServoSet * 4) + 3, 0, servoPulse[(activeServoSet * 4) + 3]);
}
void writePulses () {
servoNum = 0;
yPos = 2 + (activeServoSet * 40);
for (uint8_t i = 0; i <= 3; i ++) {
char inChar[3];
dtostrf(servoPulse[(activeServoSet * 4) + servoNum], 3, 0, inChar);
TFTscreen.fillRect(58, yPos, 30, 8, BLACK);
TFTscreen.drawString(60, yPos, inChar, YELLOW);
servoNum ++;
yPos += spacing;
}
yPos += 8;
}
void servoSetSelect() {
upButtonState = digitalRead(upButtonPin);
downButtonState = digitalRead(downButtonPin);
if (upButtonState == 1) {
activeServoSet ++;
if (activeServoSet > 3) {
activeServoSet = 0;
}
TFTscreen.fillRect(95, 0, 30, 160, BLACK);
TFTscreen.drawString(95, ((activeServoSet * 40) + 3), "<", WHITE, 4);
delay(150);
}
if (downButtonState == 1) {
activeServoSet --;
if (activeServoSet > 3) {
activeServoSet = 3;
}
tft.fillRect(95, 0, 30, 160, BLACK);
tft.drawString(95, ((activeServoSet * 40) + 3), "<", WHITE, 4);
delay(150);
}
} |
