144 lines
3.5 KiB
C++
144 lines
3.5 KiB
C++
#include <Arduino.h>
|
|
#include "espnow.h"
|
|
#include "motor.h"
|
|
#include "servo.h"
|
|
#include "fuzzy.h"
|
|
|
|
#define terminal Serial
|
|
|
|
void setup() {
|
|
|
|
// Terminal
|
|
terminal.begin(115200);
|
|
|
|
terminal.println("Hello");
|
|
|
|
// ESPNOW Communications
|
|
espnow_begin();
|
|
|
|
// Initialize Motors
|
|
motor1.begin();
|
|
motor2.begin();
|
|
|
|
// Initialize Servos
|
|
servo1.begin();
|
|
servo2.begin();
|
|
|
|
// Print Terminal Message
|
|
// terminal.println("Initialization complete.");
|
|
|
|
}
|
|
|
|
void loop() {
|
|
|
|
terminal.print(data.X); // 0% to 100%
|
|
terminal.print(" ");
|
|
terminal.print(data.Y);
|
|
terminal.print(" ");
|
|
terminal.println(data.B);
|
|
|
|
delay(20);
|
|
|
|
// // Non Blocking Motor control
|
|
// if(millis() - motorStateStartTime >= MOTOR_ACTION_DELAY) {
|
|
|
|
// motorStateStartTime = millis();
|
|
|
|
// switch(currentMotorState) {
|
|
|
|
// case STOPPED:
|
|
// Serial.println("Moving motor 1 forward...");
|
|
// motor1.forward();
|
|
// motor1.setPercentage(75); //PWM set percentage
|
|
// motor2.stop();
|
|
// currentMotorState = FORWARD_M1;
|
|
// break;
|
|
// case FORWARD_M1:
|
|
// Serial.println("Moving motor 1 backward...");
|
|
// motor1.backward();
|
|
// motor1.setPercentage(50); //PWM set percentage
|
|
// motor2.stop();
|
|
// currentMotorState = BACKWARD_M1;
|
|
// break;
|
|
// case BACKWARD_M1:
|
|
// Serial.println("Moving motor 2 forward...");
|
|
// motor1.stop();
|
|
// motor2.forward();
|
|
// motor2.setPercentage(25); //PWM set percentage
|
|
// currentMotorState = FORWARD_M2;
|
|
// break;
|
|
// case FORWARD_M2:
|
|
// Serial.println("Moving motor 2 backward...");
|
|
// motor1.stop();
|
|
// motor2.backward();
|
|
// motor2.setPercentage(100); //PWM set percentage
|
|
// currentMotorState = BACKWARD_M2;
|
|
// break;
|
|
// case BACKWARD_M2:
|
|
// Serial.println("Stopping motors ...");
|
|
// motor1.stop();
|
|
// motor2.stop();
|
|
// currentMotorState = STOPPED;
|
|
// break;
|
|
|
|
// }
|
|
|
|
// }
|
|
|
|
// // Non Blocking Servos
|
|
// static unsigned long servoTimer = 0;
|
|
// static int servo1Pos = SERVO1_START;
|
|
// static int servo2Pos = SERVO2_START;
|
|
// static bool servoForward = true;
|
|
// unsigned long currentTime = millis();
|
|
|
|
// if(currentTime - servoTimer >= 20) { // Adjust for speed
|
|
|
|
// servoTimer = currentTime;
|
|
|
|
// if (servoForward) {
|
|
// servo1Pos++;
|
|
// if (servo1Pos >= SERVO1_END) {
|
|
// servoForward = false;
|
|
// }
|
|
// } else {
|
|
// servo1Pos--;
|
|
// if (servo1Pos <= SERVO1_START) {
|
|
// servoForward = true;
|
|
// }
|
|
// }
|
|
|
|
// servo1.write(servo1Pos);
|
|
|
|
// }
|
|
|
|
// // Non Blocking Servos
|
|
// static unsigned long servoTimer2 = 0;
|
|
// static int servo2Pos2 = SERVO2_START;
|
|
// static bool servoForward2 = true;
|
|
// unsigned long currentTime2 = millis();
|
|
|
|
// if(currentTime2 - servoTimer2 >= 20) { // Adjust for speed
|
|
|
|
// servoTimer2 = currentTime2;
|
|
|
|
// if (servoForward2) {
|
|
// servo2Pos2++;
|
|
// if (servo2Pos2 >= SERVO2_END) {
|
|
// servoForward2 = false;
|
|
// }
|
|
// } else {
|
|
// servo2Pos2--;
|
|
// if (servo2Pos2 <= SERVO2_START) {
|
|
// servoForward2 = true;
|
|
// }
|
|
// }
|
|
|
|
// servo2.write(servo2Pos2);
|
|
|
|
// }
|
|
|
|
}
|
|
|
|
|
|
|