|
@@ -0,0 +1,103 @@
|
|
|
+/*
|
|
|
+
|
|
|
+ Servo Alignment Script
|
|
|
+
|
|
|
+ This script set the servo to installation positions
|
|
|
+ (as well as help debugging servo wires)
|
|
|
+
|
|
|
+
|
|
|
+*/
|
|
|
+#include <ESP32Servo.h> //Require ESP32Servo
|
|
|
+
|
|
|
+#define STP_DATA_PIN 4 // Stepper Shift Register DS
|
|
|
+#define STP_CLOCK_PIN 16 // Stepper Shift Register SH_CP
|
|
|
+#define STP_LATCH_PIN 17 // Stepper Shift Register ST_CP
|
|
|
+
|
|
|
+#define SERVO_SWITCH 27 //Servo to push the switch
|
|
|
+#define SERVO_COVER 14 //Servo to push the cover
|
|
|
+
|
|
|
+#define TOGGLE_SWITCH 13 //Switch on top of the matrix display
|
|
|
+#define SERVO_ALIGNMENT_OFFSET 3
|
|
|
+
|
|
|
+/* Hardware Type Definations */
|
|
|
+Servo servoSwitchPusher;
|
|
|
+Servo servoCoverPusher;
|
|
|
+
|
|
|
+//Get the current state of the switch
|
|
|
+// true = After human pushed
|
|
|
+// false = After robot pushed
|
|
|
+bool getSwitchState() {
|
|
|
+ int switchState = digitalRead(TOGGLE_SWITCH);
|
|
|
+ return (switchState == 1);
|
|
|
+}
|
|
|
+
|
|
|
+//Push switch back with delay
|
|
|
+void pushSwitchDelayed(int coverDelay, int pusherDelay) {
|
|
|
+ servoCoverPusher.write(90);
|
|
|
+ delay(coverDelay);
|
|
|
+ servoSwitchPusher.write(130 + SERVO_ALIGNMENT_OFFSET);
|
|
|
+ delay(pusherDelay);
|
|
|
+ servoCoverPusher.write(0);
|
|
|
+ servoSwitchPusher.write(0);
|
|
|
+}
|
|
|
+
|
|
|
+//writeStep write the step given the byte representation of a step on both stepper
|
|
|
+//e.g. writeStep(0b00110011);
|
|
|
+void writeStep(byte stepByte) {
|
|
|
+ shiftOut(STP_DATA_PIN, STP_CLOCK_PIN, MSBFIRST, stepByte);
|
|
|
+ digitalWrite(STP_LATCH_PIN, HIGH);
|
|
|
+ digitalWrite(STP_LATCH_PIN, LOW);
|
|
|
+ delayMicroseconds(840); //780 - 800 ms minimum, recommend 840 to 860ms
|
|
|
+}
|
|
|
+
|
|
|
+//Set all coils to off and enter standby mode
|
|
|
+//to save power and prevent overheating
|
|
|
+void standbySteppers() {
|
|
|
+ writeStep(0b00000000);
|
|
|
+}
|
|
|
+
|
|
|
+void setup() {
|
|
|
+ Serial.begin(115200);
|
|
|
+
|
|
|
+ // Allow allocation of all timers
|
|
|
+ ESP32PWM::allocateTimer(0);
|
|
|
+ ESP32PWM::allocateTimer(1);
|
|
|
+ ESP32PWM::allocateTimer(2);
|
|
|
+ ESP32PWM::allocateTimer(3);
|
|
|
+
|
|
|
+ // put your setup code here, to run once:
|
|
|
+ /* Servo IO */
|
|
|
+ servoSwitchPusher.setPeriodHertz(50);
|
|
|
+ servoCoverPusher.setPeriodHertz(50);
|
|
|
+ servoSwitchPusher.attach(SERVO_SWITCH);
|
|
|
+ servoCoverPusher.attach(SERVO_COVER);
|
|
|
+ servoCoverPusher.write(0);
|
|
|
+ servoSwitchPusher.write(0);
|
|
|
+ //Set servo pusher to vertical install position
|
|
|
+ delay(1000);
|
|
|
+ servoSwitchPusher.write(125);
|
|
|
+
|
|
|
+ /* Stepper IO */
|
|
|
+ pinMode(STP_DATA_PIN, OUTPUT);
|
|
|
+ pinMode(STP_CLOCK_PIN, OUTPUT);
|
|
|
+ pinMode(STP_LATCH_PIN, OUTPUT);
|
|
|
+ standbySteppers();
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void loop() {
|
|
|
+ // put your main code here, to run repeatedly:
|
|
|
+ //Switch test
|
|
|
+ bool switchPushed = getSwitchState();
|
|
|
+ if (switchPushed) {
|
|
|
+ servoSwitchPusher.write(0);
|
|
|
+ Serial.println("Switch pushed");
|
|
|
+ delay(1000);
|
|
|
+ pushSwitchDelayed(300,300);
|
|
|
+ delay(3000);
|
|
|
+ servoSwitchPusher.write(125);
|
|
|
+ } else {
|
|
|
+ Serial.println("Switch idle");
|
|
|
+ }
|
|
|
+ delay(500);
|
|
|
+}
|