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);
}
} |
Partager