This is a long shot because its a super long code file but can someone please explain why my code is not making the motors run?
All components run independently. Merged code and it stopped running.
// Load Libraries
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
//#include <Button.h>
#include <TimerOne.h>
#include <Encoder.h>
// Create class
#define OLED_RESET -1
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
Adafruit_MPU6050 mpu;
#define calibButton 5
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
// SDA 20
// SCLK 21
#define NPN_PIN 11
// Rotary Encoder Inputs
#define RE_1_outputCLK 2 // RE = Rotary Encoder
#define RE_1_outputDT 3 // RE = Rotary Encoder
#define RE_2_outputCLK 8 // RE = Rotary Encoder
#define RE_2_outputDT 9 // RE = Rotary Encoder
#define ForwardOrReverseButton 6 //only engages reverse when button is activated (LOW; pressed in)
#define Motor1Direction 4 // Motor Direction Control Pin with PWM
#define Motor2Direction 10 // Motor Direction Control Pin with PWM
#define Motor1Speed 12 // Blue wire, PWM
#define Motor2Speed 13 // Green wire, PWM
int RE_1_counter = 0; // Increment counter on the rotary encoder for motor 1
int RE_2_counter = 0; // Increment counter on the rotary encoder for motor 2
int RE_1_State; // RE Rotation Counter on motor 1
int RE_1_LastState; // RE Rotation Memory Location on motor 2
int RE_2_State; // RE Rotation Counter on motor 1
int RE_2_LastState; // RE Rotation Memory Location on motor 2
int NoSpeed = 0; // No motor speed
int PauseBeforeCountingOnEncodersResumes = 2000;
int clickNum = 360;
const int trigger = 15, calTrigger = trigger * 8;// Basic rotary encoders for Arduino are 30 steps per rev; 1/4 turn
String currDir1 ="", prevDir1 ="Idle", mode = "Calibration";
String currDir2 ="", prevDir2 ="Idle", calib1 = "No", calib2 = "No";
int mainWheelRadiusIn = 24; // In
float mainWheelRadiusM = mainWheelRadiusIn * 0.254; // Meters
float angle1;
float angle2;
unsigned long currMillis, startTime1, finalTime1, currDur1, btDur1, totDur1;
unsigned long startTime2, finalTime2, currDur2, btDur2, totDur2;
const long pauseDuration = 2000; // Miliseconds
const long motorInterval = 3000; // Milliseconds
float angAcceli1, angAccelm1, angAccelf1, angVelf1, avgAngAccel1;
float angAcceli2, angAccelm2, angAccelf2, angVelf2, avgAngAccel2;
float maxUserSpeed1 = 1.7, currUserSpeed1, speedRatio1;
float maxUserSpeed2 = 1.7, currUserSpeed2, speedRatio2;
int maxMotorSpeedFw1 = 50, maxMotorSpeedRv1 = 25, currMotorSpeed1, currMotorSpeed2;
int maxMotorSpeedFw2 = maxMotorSpeedFw1, maxMotorSpeedRv2 = maxMotorSpeedRv1;
// Vars for Sensor Data
sensors_event_t a, g, temp;
const int numSamples = 1000; // Number of samples to collect for calibration
const int calibrateDelay = 10; // Delay in milliseconds between samples
float axOffset = 0, ayOffset = 0, azOffset = 0; // Calibration offsets for accelerometer
float axCalibrated = 0, ayCalibrated = 0, azCalibrated = 0;
double tiltX = 0, tiltY = 0, tiltZ = 0;
const unsigned long motorDuration = motorInterval; // Adjust the duration as needed; used to be 3000
//motorDuration defines how long the motors can run in one direction
float maxTilt = 10;
Encoder encoder1(RE_1_outputCLK, RE_1_outputDT);
Encoder encoder2(RE_2_outputCLK, RE_2_outputDT);
// Define the states for your state machine
enum MotorState {
IDLE,
MOTOR1_FORWARD,
MOTOR1_REVERSE,
MOTOR2_FORWARD,
MOTOR2_REVERSE
};
// Motors have no speed or direction, the motors are stationary
// initial set up state
MotorState motor1State = IDLE;
MotorState motor2State = IDLE;
void calibrateMPU(){
float axSum = 0, aySum = 0, azSum = 0;
robojaxText("Calibrating MPU6050. Please keep the sensor still...", 4, 3, 1,true);
delay(calibrateDelay); // Initial delay before starting calibration
for (int i = 0; i < numSamples; i++) {
// Get new sensor events with the readings
mpu.getEvent(&a, &g, &temp);
axSum += a.acceleration.x;
aySum += a.acceleration.y;
azSum += a.acceleration.z;
delay(calibrateDelay);
}
axOffset = axSum / numSamples;
ayOffset = aySum / numSamples;
azOffset = azSum / numSamples;
robojaxText("MPU Calibration complete!",4,3,1,true);
delay(2000);
}
void getMPUData(){
mpu.getEvent(&a, &g, &temp);
axCalibrated = a.acceleration.x - axOffset;
ayCalibrated = a.acceleration.y - ayOffset;
azCalibrated = a.acceleration.z - azOffset;
tiltX = atan2(ayCalibrated, azCalibrated) * (180.0 / PI);
tiltY = atan2(axCalibrated, azCalibrated) * (180.0 / PI);
tiltZ = atan2(ayCalibrated, axCalibrated) * (180.0 / PI);
}
void switchPanel(){
// Write to the NPN transistor
float duty = 0.5;
Timer1.pwm(NPN_PIN, duty*1023);
analogWrite(NPN_PIN, duty*255); //input pwm signal to switch to the transistor
//Read Voltage
float vA0 = analogRead(A0);
float vOut = vA0 * 5.0 / 1023.0 * (5.0 + 1.0) / 1.0; //voltage read calculation
}
void resetCounter1(){
if(mode == "Calibration"){
if ((abs(RE_1_counter) >= calTrigger) || (currDir1 != prevDir1) || (btDur1 >= pauseDuration)){
RE_1_counter = 0;
}
return;
}
if ((abs(RE_1_counter) >= trigger) || (currDir1 != prevDir1) || (btDur1 >= pauseDuration)){
RE_1_counter = 0;
}
return;
}
void resetCounter2(){
if(mode == "Calibration"){
if ((abs(RE_2_counter) >= calTrigger) || (currDir2 != prevDir2) || (btDur2 >= pauseDuration)){
RE_2_counter = 0;
}
return;
}
if ((abs(RE_2_counter) >= trigger) || (currDir2 != prevDir2) || (btDur2 >= pauseDuration)){
RE_2_counter = 0;
}
return;
}
float getAngAccel(){
getMPUData();
// Calculation
float angAccel = axCalibrated / mainWheelRadiusM;
return angAccel;
}
float getAngVel( float angAccel, float finalAng){
float angVel = sqrt(2 * angAccel * (finalAng));
return angVel;
}
float getLinVel(float angVel, float mainWheelRadiusM){
float linVel = angVel * mainWheelRadiusM;
return linVel;
}
/*
* robojaxText(String text, int x, int y,int size, boolean d)
* text is the text string to be printed
* x is the integer x position of text
* y is the integer y position of text
* z is the text size, 1, 2, 3 etc
* d is either "true" or "false". Not sure, use true
*/
void robojaxText(String text, int x, int y,int size, bool d) {
display.setTextSize(size);
display.setTextColor(WHITE,BLACK);
display.setCursor(x,y);
display.println(text);
if (d){
display.display();
display.fillScreen(BLACK);
}
}
void readTime(){
// Read time
currMillis = millis();
btDur1 = currMillis - finalTime1;
currDur1 = currMillis - startTime1;
totDur1 = finalTime1 - startTime1;
btDur2 = currMillis - finalTime2;
currDur2 = currMillis - startTime2;
totDur2 = finalTime2 - startTime2;
}
void calibrateEncoders(){
readTime();
// Read the current state of CLK
RE_1_State = digitalRead(RE_1_outputCLK);
if (RE_1_State != RE_1_LastState){
// If the DT state is different than the CLK state then
// the encoder is rotating CCW so decrement
if (digitalRead(RE_1_outputDT) != RE_1_State) {
RE_1_counter --;
currDir1 ="Backward";
}
else {
// Encoder is rotating CW so increment
RE_1_counter ++;
currDir1 ="Forward";
}
angle1 = RE_1_counter * (360.00 / clickNum);
if (currDir1 != prevDir1){
startTime1 = currMillis;
angAcceli1 = getAngAccel();
finalTime1 = startTime1;
}
if (RE_1_counter == (calTrigger/2)){
angAccelm1 = getAngAccel();
}
else if (RE_1_counter <= (calTrigger)){
finalTime1 = currMillis;
angAccelf1 = getAngAccel();
avgAngAccel1 = (angAcceli1 + angAccelf1 + angAccelm1) / 3;
angVelf1 = getAngVel(avgAngAccel1, angle1);
maxUserSpeed1 = getLinVel(angVelf1, mainWheelRadiusM);
calib1 = "Yes";
}
else if (currDir1 == "Backwards" && prevDir1 == "Backwards"){
robojaxText("Please try again. Push Forward.",4,3,1,true);
delay(2000);
RE_1_counter = calTrigger + 2;
resetCounter1();
}
else if (btDur1 >= pauseDuration) {
robojaxText("Please try again...",4,3,1,true);
delay(2000);
RE_1_counter = (calTrigger + 1);
resetCounter1();
}
else {
robojaxText("Keep going!",4,3,1,true);
}
// Put in a slight delay to help debounce the reading
delay(10);
resetCounter1();
}
// Remember last CLK state
RE_1_LastState = RE_1_State;
prevDir1 = currDir1;
readTime();
// Read the current state of CLK
RE_2_State = digitalRead(RE_2_outputCLK);
if (RE_2_State != RE_2_LastState){
// If the DT state is different than the CLK state then
// the encoder is rotating CCW so decrement
if (digitalRead(RE_2_outputDT) != RE_2_State) {
RE_2_counter --;
currDir2 ="Backward";
}
else {
// Encoder is rotating CW so increment
RE_2_counter ++;
currDir2 ="Forward";
}
angle2 = RE_2_counter * (360.00 / clickNum);
if (currDir2 != prevDir2){
startTime2 = currMillis;
angAcceli2 = getAngAccel();
finalTime2 = startTime2;
}
if (RE_2_counter == (calTrigger/2)){
angAccelm2 = getAngAccel();
}
else if (RE_2_counter <= (calTrigger)){
finalTime2 = currMillis;
angAccelf2 = getAngAccel();
avgAngAccel2 = (angAcceli2 + angAccelf2 + angAccelm2) / 3;
angVelf2 = getAngVel(avgAngAccel2, angle2);
maxUserSpeed2 = getLinVel(angVelf2, mainWheelRadiusM);
calib2 = "Yes";
}
else if (currDir2 == "Backwards" && prevDir2 == "Backwards"){
robojaxText("Please try again. Push Forward.",4,3,1,true);
delay(2000);
RE_2_counter = calTrigger + 2;
resetCounter2();
}
else if (btDur2 >= pauseDuration) {
robojaxText("Please try again...",4,3,1,true);
delay(2000);
RE_2_counter = (calTrigger + 1);
resetCounter2();
}
else {
robojaxText("Keep going!",4,3,1,true);
}
// Put in a slight delay to help debounce the reading
delay(10);
resetCounter2();
}
// Remember last CLK state
RE_2_LastState = RE_2_State;
prevDir2 = currDir2;
if((calib1 == "Yes") && (calib2 == "Yes")){
mode = "Drive";
}
}
void calibration() {
for (int i = 5; i >= 0; --i){
robojaxText("Calibrating MPU, Keep still in",4,3,1,false);
robojaxText(String(i), 4, 21, 1, true);
delay(1000);
}
calibrateMPU();
robojaxText("Please Push Forward as hard as you can...", 4, 3, 1, true);
delay(1000);
calib1 = "No";
calib2 = "No";
while (mode == "Calibration"){
calibrateEncoders();
}
resetCounter1();
return;
}
void willMotorStart(){
readTime();
// Read the current state of CLK
RE_1_State = digitalRead(RE_1_outputCLK);
// If last and current state of CLK are different, then pulse occurred
// React to only 1 state change to avoid double count
if (RE_1_State != RE_1_LastState){
// If the DT state is different than the CLK state then
// the encoder is rotating CCW so decrement
if (digitalRead(RE_1_outputDT) != RE_1_State) {
RE_1_counter --;
currDir1 ="Backward";
}
else {
// Encoder is rotating CW so increment
RE_1_counter ++;
currDir1 ="Forward";
}
}
// Read the current state of CLK
RE_2_State = digitalRead(RE_2_outputCLK);
// If last and current state of CLK are different, then pulse occurred
// React to only 1 state change to avoid double count
if (RE_2_State != RE_2_LastState){
// If the DT state is different than the CLK state then
// the encoder is rotating CCW so decrement
if (digitalRead(RE_2_outputDT) != RE_2_State) {
RE_2_counter --;
currDir2 ="Backward";
}
else {
// Encoder is rotating CW so increment
RE_2_counter ++;
currDir2 ="Forward";
}
}
if (abs(RE_1_counter - RE_2_counter) >= (trigger*2))
{
mode == "Turn";
}
angle1 = RE_1_counter * (360.00 / clickNum);
if (currDir1 != prevDir1){
startTime1 = currMillis;
angAcceli1 = getAngAccel();
finalTime1 = startTime1;
}
else if (RE_1_counter == abs(trigger/2)){
angAccelm1 = getAngAccel();
}
else if (RE_1_counter <= abs(trigger)){
finalTime1 = currMillis;
angAccelf1 = getAngAccel();
}
else if (btDur1 >= pauseDuration) {
currDir1 = 'Idle';
resetCounter1();
}
if (RE_1_counter >= trigger)
{
avgAngAccel1 = (angAcceli1 + angAccelf1 + angAccelm1) / 3;
angVelf1 = getAngVel(avgAngAccel1, angle1);
currUserSpeed1 = getLinVel(angVelf1, mainWheelRadiusM);
speedRatio1 = currUserSpeed1 / maxUserSpeed1;
if (speedRatio1 >= 1){
currMotorSpeed1 = maxMotorSpeedFw1;
}
else{
currMotorSpeed1 = speedRatio1 * maxMotorSpeedFw1;
}
if (tiltX > 0 && tiltX < maxTilt) {
currMotorSpeed1 += tiltX;
}
else if (tiltX > 0 && abs(tiltX) >= maxTilt) {
currMotorSpeed1 += maxTilt;
}
if (abs(tiltY) < maxTilt) {
currMotorSpeed1 += tiltY;
}
else if (tiltY > 0 && abs(tiltY) >= maxTilt) {
currMotorSpeed1 += maxTilt;
}
if (motor1State == IDLE){
handleMotor1(RE_1_counter);
}
}
else if (RE_1_counter <= -trigger){
angVelf1 = getAngVel(abs(avgAngAccel1), abs(angle1));
currUserSpeed1 = getLinVel(angVelf1, mainWheelRadiusM);
speedRatio1 = currUserSpeed1 / maxUserSpeed1;
if (speedRatio1 >= 1){
currMotorSpeed1 = maxMotorSpeedRv1;
}
else{
currMotorSpeed1 = speedRatio1 * maxMotorSpeedRv1;
}
if (motor1State == IDLE){
handleMotor1(RE_1_counter);
}
}
angle2 = RE_2_counter * (360.00 / clickNum);
if (currDir2 != prevDir2){
startTime2 = currMillis;
angAcceli2 = getAngAccel();
finalTime2 = startTime2;
}
else if (RE_1_counter == abs(trigger/2)){
angAccelm2 = getAngAccel();
}
else if (RE_2_counter <= abs(trigger)){
finalTime2 = currMillis;
angAccelf2 = getAngAccel();
}
else if (btDur2 >= pauseDuration) {
currDir2 = 'Idle';
resetCounter2();
}
if (RE_2_counter >= trigger)
{
avgAngAccel2 = (angAcceli2 + angAccelf2 + angAccelm2) / 3;
angVelf2 = getAngVel(avgAngAccel2, angle2);
currUserSpeed1 = getLinVel(angVelf2, mainWheelRadiusM);
speedRatio2 = currUserSpeed2 / maxUserSpeed2;
if (speedRatio2 >= 1){
currMotorSpeed2 = maxMotorSpeedFw2;
}
else{
currMotorSpeed2 = speedRatio2 * maxMotorSpeedFw2;
}
if (tiltX > 0 < maxTilt) {
currMotorSpeed2 += tiltX;
}
else if (tiltX < 0 && abs(tiltX) >= maxTilt) {
currMotorSpeed2 += maxTilt;
}
if (abs(tiltY) < maxTilt) {
currMotorSpeed2 += tiltY;
}
else if (tiltY > 0 && abs(tiltY) >= maxTilt) {
currMotorSpeed1 += maxTilt;
}
if (motor2State == IDLE){
handleMotor2(RE_2_counter);
}
}
else if (RE_2_counter <= -trigger){
angVelf2 = getAngVel(abs(avgAngAccel2), abs(angle2));
currUserSpeed2 = getLinVel(angVelf2, mainWheelRadiusM);
speedRatio2 = currUserSpeed2 / maxUserSpeed2;
if (speedRatio2 >= 1){
currMotorSpeed2 = maxMotorSpeedRv2;
}
else{
currMotorSpeed2 = speedRatio2 * maxMotorSpeedRv2;
}
if (motor2State == IDLE){
handleMotor2(RE_2_counter);
}
}
// Put in a slight delay to help debounce the reading
delay(1);
resetCounter1();
delay(1);
resetCounter2();
// Remember last CLK state
RE_1_LastState = RE_1_State;
prevDir1 = currDir1;
RE_2_LastState = RE_2_State;
prevDir2 = currDir2;
}
void handleMotor1(int RE_1_counter){
int buttonState = digitalRead(ForwardOrReverseButton); //reads status of the forward or reverse switch
if (mode == "Turn")
{
if (RE_1_counter - RE_2_counter > 0){
startMotor1Forward(); //right motor forward
}
else if (RE_1_counter - RE_2_counter < 0){
startMotor1Reverse(); //right motor back
}
return;
}
switch (RE_1_counter)
{
case -trigger: //trigger value on rotary encoder
if (buttonState == LOW) //reverse is active
{
startMotor1Reverse();
break;
}
else
{
break;
}
case trigger:
if (buttonState == HIGH) //forward is active
{
startMotor1Forward();
break;
}
else
{
break;
}
default:
break;
}
}
void handleMotor2(int RE_2_counter){
int buttonState = digitalRead(ForwardOrReverseButton); //reads status of the forward or reverse switch
if (mode == "Turn")
{
if (RE_1_counter - RE_2_counter > 0){
startMotor2Reverse(); //right motor forward
}
else if (RE_1_counter - RE_2_counter < 0){
startMotor2Forward(); // left turn
}
return;
}
switch (RE_2_counter)
{
case -trigger:
if (buttonState == LOW) //reverse is active
{
startMotor2Reverse();
break;
}
else
{
break;
}
case trigger:
if (buttonState == HIGH) //forward is active
{
startMotor2Forward();
break;
}
else
{
break;
}
break;
default:
break;
}
}
void startMotor1Forward()
{
motor1State = MOTOR1_FORWARD;
digitalWrite(Motor1Direction, LOW);
analogWrite(Motor1Speed, currMotorSpeed1);
startTime1 = millis();
resetCountersAndDelay1();
}
void startMotor1Reverse()
{
motor1State = MOTOR1_REVERSE;
digitalWrite(Motor1Direction, HIGH);
analogWrite(Motor1Speed, currMotorSpeed1);
startTime1 = millis();
resetCountersAndDelay1();
}
void startMotor2Forward()
{
motor2State = MOTOR2_FORWARD;
digitalWrite(Motor2Direction, HIGH);
analogWrite(Motor2Speed, currMotorSpeed2);
startTime2 = millis();
resetCountersAndDelay2();
}
void startMotor2Reverse()
{
motor2State = MOTOR2_REVERSE;
digitalWrite(Motor2Direction, LOW);
analogWrite(Motor2Speed, currMotorSpeed2);
startTime2 = millis();
resetCountersAndDelay2();
}
void resetCountersAndDelay1() {
RE_1_counter = 0; // Reset Motor 1 counter
//RE_2_counter = 0; // Reset Motor 2 counter
delay(PauseBeforeCountingOnEncodersResumes); // Delay for 2 seconds
// After resetting counters and the delay, return to the loop
return;
}
void resetCountersAndDelay2() {
//RE_1_counter = 0; // Reset Motor 1 counter
RE_2_counter = 0; // Reset Motor 2 counter
delay(PauseBeforeCountingOnEncodersResumes); // Delay for 2 seconds
// After resetting counters and the delay, return to the loop
return;
}
void setup(){
// Setup Serial Monitor
Serial.begin(9600);
Wire.begin();
pinMode(RE_1_outputDT, INPUT);
pinMode(RE_2_outputDT, INPUT);
pinMode(Motor1Speed, OUTPUT);
pinMode(Motor2Speed, OUTPUT);
pinMode(Motor1Direction, OUTPUT);
pinMode(Motor2Direction, OUTPUT);
pinMode(ForwardOrReverseButton, INPUT_PULLUP);
pinMode(calibButton, INPUT_PULLUP);
RE_1_LastState = digitalRead(RE_1_outputCLK);
RE_2_LastState = digitalRead(RE_2_outputCLK);
display.begin(SSD1306_SWITCHCAPVCC, 0x3C, false, false);
display.display();
delay(1000);
display.clearDisplay();
// Initialize mpu;
if (!mpu.begin()) {
Serial.println(F("Failed to find MPU6050 chip"));
while (1) {
delay(10);
}
}
Serial.println(F("MPU6050 Found!"));
//Set accelerometer range and filter bandwidth (optional, based on your requirements)
mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
mpu.setGyroStandby(true, true, true);
mpu.setTemperatureStandby(true);
calibration();
//Timer1 setup
Timer1.initialize(100); // 100 us => 10kHz
Timer1.pwm(NPN_PIN,0); //Start with 0 duty
// Changed from 19200 to 9600 which seems to be default for Arduino serial monitor
}
void loop() {
readTime();
mode = "Drive";
robojaxText("Steering Mode Engaged!",4,3,1,true);
if (motor1State != IDLE && currMillis - startTime1 >= motorDuration)
{//initial state of motors is to remain idle until action
motor1State = IDLE;
digitalWrite(Motor1Speed, NoSpeed);
digitalWrite(Motor1Direction, HIGH);
}
if (motor2State != IDLE && currMillis - startTime2 >= motorDuration)
{//initial state of motors is to remain idle until action
motor2State = IDLE;
digitalWrite(Motor2Speed, NoSpeed);
digitalWrite(Motor2Direction, HIGH);
}
int calibButtonState = digitalRead(calibButton);
willMotorStart();
if (calibButtonState == HIGH){
mode = "Calibration";
calibration();
}
}