Hey All, not sure if anyone else on here has ever tackled the HowToMechatronics.com Mars Rover project but I'm nearly complete though running into some code issues when trying to upload to the Mega. The code is admittedly my weak point, especially when viewing code that someone else has written that may need slight debugging. Was able to 3D print, build, assemble, wire up, solder, etc. this whole thing and hoping to be able to take it to a Cub Scout rocket launch event this weekend. At this point I have added all the proper libraries but am getting stuck on, what seem like, a small issue of an undefined variable (i.e. "not declared in this scope" error). I'm guessing there might be other errors after this, but I get stuck at this part when uploading the code. Any guidance for someone that is more than willing to try to learn would be appreciated. Below is the code, forgive me if there is a better way to upload code like this that's not quite so bulky.
#include <Servo.h>
#include <ServoEasing.h>
#include <IBusBM.h>
#include <AccelStepper.h>
#define motorW1_IN1 6
#define motorW1_IN2 7
#define motorW2_IN1 4
#define motorW2_IN2 5
#define motorW3_IN1 2
#define motorW3_IN2 3
#define motorW4_IN1 13
#define motorW4_IN2 10
#define motorW5_IN1 8
#define motorW5_IN2 9
#define motorW6_IN1 11
#define motorW6_IN2 12
ServoEasing servoW1;
ServoEasing servoW3;
ServoEasing servoW4;
ServoEasing servoW6;
ServoEasing servoCamTilt;
AccelStepper camPanStepper(1, 46, 45); //(Type:driver, STEP, DIR) - Stepper1
IBusBM IBus;
IBusBM IBusSensor;
int angle = 0; // servo position in degrees
int ch0, ch1, ch2, ch3, ch6 = 0;
int servo1Angle = 90;
int servo3Angle = 90;
int servo4Angle = 90;
int servo6Angle = 90;
int s = 0; // rover speed
int r = 0; // turning radius
int m1, m2, m3, m4, m5, m6;
int camTilt = 90;
int camPan = 0;
float speed1, speed2, speed3 = 0;
float speed1PWM, speed2PWM, speed3PWM = 0;
float thetaInnerFront, thetaInnerBack, thetaOuterFront, thetaOuterBack = 0;
float d1 = 271; // distance in mm
float d2 = 278;
float d3 = 301;
float d4 = 304;
void setup() {
/*
Use this if you need to change the frequency of the PWM signals
TCCR4B = TCCR4B & B11111000 | B00000101; // D6,D7,D8 PWM frequency of 30.64 Hz
TCCR2B = TCCR2B & B11111000 | B00000111; // D9, D10 PWM frequency of 30.64 Hz
TCCR1B = TCCR1B & B11111000 | B00000101; // D11, D12 PWM frequency of 30.64 Hz
TCCR5B = TCCR5B & B11111000 | B00000101; // D4, D13 PWM frequency of 30.64 Hz
TCCR3B = TCCR3B & B11111000 | B00000101; // D2, D3, D5 PWM frequency of 30.64 Hz
*/
Serial.begin(115200);
IBus.begin(Serial1, IBUSBM_NOTIMER); // Servo iBUS
IBusSensor.begin(Serial2, IBUSBM_NOTIMER); // Sensor iBUS
IBusSensor.addSensor(IBUSS_INTV); // add voltage sensor
servoW1.attach(22);
servoW3.attach(23);
servoW4.attach(24);
servoW6.attach(25);
servoCamTilt.attach(26);
servoW1.write(90);
servoW3.write(90);
servoW4.write(90);
servoW6.write(90);
servoCamTilt.write(90);
servoW1.setSpeed(550);
servoW3.setSpeed(550);
servoW4.setSpeed(550);
servoW6.setSpeed(550);
servoCamTilt.setSpeed(200);
camPanStepper.setMaxSpeed(1000);
camPan = 0;
camTilt = 90;
// DC Motors
// Motor Wheel 1 - Left Front
digitalWrite(motorW1_IN1, LOW); // PWM value
digitalWrite(motorW1_IN2, LOW); // Forward
// Motor Wheel 2 - Left Middle
digitalWrite(motorW2_IN1, LOW);
digitalWrite(motorW2_IN2, LOW);
// Motor Wheel 3 - Left Back
digitalWrite(motorW3_IN1, LOW);
digitalWrite(motorW3_IN2, LOW);
// right side motors move in opposite direction
// Motor Wheel 4 - Right Front
digitalWrite(motorW4_IN1, LOW);
digitalWrite(motorW4_IN2, LOW);
// Motor Wheel 5 - Right Middle
digitalWrite(motorW5_IN1, LOW);
digitalWrite(motorW5_IN2, LOW);
// Motor Wheel 6 - Right Back
digitalWrite(motorW6_IN1, LOW);
digitalWrite(motorW6_IN2, LOW);
}
void loop() {
// Reading the data comming from the RC Transmitter
IBus.loop();
ch0 = IBus.readChannel(0);
ch1 = IBus.readChannel(1);
ch2 = IBus.readChannel(2);
ch3 = IBus.readChannel(3);
ch6 = IBus.readChannel(6);
// Convertign the incoming data
// Steering right
if (ch0 > 1515) {
r = map(ch0, 1515, 2000, 1400, 600); // turining radius from 1400mm to 600mm
}
// Steering left
else if (ch0 < 1485) {
r = map(ch0, 1485, 1000, 1400, 600); // turining radius from 600mm to 1400mm
}
// Rover speed in % from 0 to 100
s = map(ch2, 1000, 2000, 0, 100); // rover speed from 0% to 100%
// Camera head steering
if (ch1 < 1485 ) {
if (camTilt >= 35) {
camTilt--;
delay(20);
}
}
if (ch1 > 1515 ) {
if (camTilt <= 165) {
camTilt++;
delay(20);
}
}
servoCamTilt.startEaseTo(camTilt); // Camera tilt
if (ch3 >= 1000 && ch3 < 1485) {
camPan = map(ch3, 1000, 1485, 400, 0);
}
else if (ch3 > 1515 && ch3 <= 2000) {
camPan = map(ch3, 1515, 2000, 0, -400);
}
else {
camPan = 0;
}
camPanStepper.setSpeed(camPan); // Camera pan
camPanStepper.run();
calculateMotorsSpeed();
calculateServoAngle();
// Steer right
if (ch0 > 1515) {
// Servo motors
// Outer wheels
servoW1.startEaseTo(97 + thetaInnerFront); // front wheel steer right
servoW3.startEaseTo(97 - thetaInnerBack); // back wheel steer left for overall steering to the right of the rover
// Inner wheels
servoW4.startEaseTo(94 + thetaOuterFront);
servoW6.startEaseTo(96 - thetaOuterBack);
// DC Motors
if (ch6 < 1500) { // Move forward
// Motor Wheel 1 - Left Front
analogWrite(motorW1_IN1, speed1PWM); // Outer wheels running at speed1 - max speed
digitalWrite(motorW1_IN2, LOW);
// Motor Wheel 2 - Left Middle
analogWrite(motorW2_IN1, speed1PWM);
digitalWrite(motorW2_IN2, LOW);
// Motor Wheel 3 - Left Back
analogWrite(motorW3_IN1, speed1PWM);
digitalWrite(motorW3_IN2, LOW);
// right side motors move in opposite direction
// Motor Wheel 4 - Right Front
digitalWrite(motorW4_IN1, LOW);
analogWrite(motorW4_IN2, speed2PWM); // Inner front wheel running at speed2 - lower speed
// Motor Wheel 5 - Right Middle
digitalWrite(motorW5_IN1, LOW);
analogWrite(motorW5_IN2, speed3PWM); // Inner middle wheel running at speed3 - lowest speed
// Motor Wheel 6 - Right Back
digitalWrite(motorW6_IN1, LOW);
analogWrite(motorW6_IN2, speed2PWM); // Inner back wheel running at speed2 - lower speed
}
else if (ch6 > 1500) {
// Motor Wheel 1 - Left Front
digitalWrite(motorW1_IN1, LOW); // Outer wheels running at speed1 - max speed
analogWrite(motorW1_IN2, speed1PWM);
// Motor Wheel 2 - Left Middle
digitalWrite(motorW2_IN1, LOW);
analogWrite(motorW2_IN2, speed1PWM);
// Motor Wheel 3 - Left Back
digitalWrite(motorW3_IN1, LOW);
analogWrite(motorW3_IN2, speed1PWM);
// right side motors move in opposite direction
// Motor Wheel 4 - Right Front
analogWrite(motorW4_IN1, speed2PWM);
digitalWrite(motorW4_IN2, LOW); // Inner front wheel running at speed2 - lower speed
// Motor Wheel 5 - Right Middle
analogWrite(motorW5_IN1, speed3PWM);
digitalWrite(motorW5_IN2, LOW); // Inner middle wheel running at speed3 - lowest speed
// Motor Wheel 6 - Right Back
analogWrite(motorW6_IN1, speed2PWM);
digitalWrite(motorW6_IN2, LOW); // Inner back wheel running at speed2 - lower speed
}
}
// Steer left
else if (ch0 < 1485) {
// Servo motors
servoW1.startEaseTo(97 - thetaOuterFront);
servoW3.startEaseTo(97 + thetaOuterBack);
servoW4.startEaseTo(94 - thetaInnerFront);
servoW6.startEaseTo(96 + thetaInnerBack);
// DC Motors
if (ch6 < 1500) { // Move forward
// Motor Wheel 1 - Left Front
analogWrite(motorW1_IN1, speed2PWM); // PWM value
digitalWrite(motorW1_IN2, LOW); // Forward
// Motor Wheel 2 - Left Middle
analogWrite(motorW2_IN1, speed3PWM);
digitalWrite(motorW2_IN2, LOW);
// Motor Wheel 3 - Left Back
analogWrite(motorW3_IN1, speed2PWM);
digitalWrite(motorW3_IN2, LOW);
// Motor Wheel 4 - Right Front
// right side motors move in opposite direction
digitalWrite(motorW4_IN1, LOW);
analogWrite(motorW4_IN2, speed1PWM);
// Motor Wheel 5 - Right Middle
digitalWrite(motorW5_IN1, LOW);
analogWrite(motorW5_IN2, speed1PWM);
// Motor Wheel 6 - Right Back
digitalWrite(motorW6_IN1, LOW);
analogWrite(motorW6_IN2, speed1PWM);
}
else if (ch6 > 1500) { // Move backward
// Motor Wheel 1 - Left Front
digitalWrite(motorW1_IN1, LOW); // PWM value
analogWrite(motorW1_IN2, speed2PWM); // Forward
// Motor Wheel 2 - Left Middle
digitalWrite(motorW2_IN1, LOW);
analogWrite(motorW2_IN2, speed3PWM);
// Motor Wheel 3 - Left Back
digitalWrite(motorW3_IN1, LOW);
analogWrite(motorW3_IN2, speed2PWM);
// Motor Wheel 4 - Right Front
// right side motors move in opposite direction
analogWrite(motorW4_IN1, speed1PWM);
digitalWrite(motorW4_IN2, LOW);
// Motor Wheel 5 - Right Middle
analogWrite(motorW5_IN1, speed1PWM);
digitalWrite(motorW5_IN2, LOW);
// Motor Wheel 6 - Right Back
analogWrite(motorW6_IN1, speed1PWM);
digitalWrite(motorW6_IN2, LOW);
}
}
// Move straight
else {
servoW1.startEaseTo(97);
servoW3.startEaseTo(97);
servoW4.startEaseTo(94);
servoW6.startEaseTo(96);
// DC Motors
if (ch6 < 1500) {
// Motor Wheel 1 - Left Front
analogWrite(motorW1_IN1, speed1PWM); // all wheels move at the same speed
digitalWrite(motorW1_IN2, LOW); // Forward
// Motor Wheel 2 - Left Middle
analogWrite(motorW2_IN1, speed1PWM);
digitalWrite(motorW2_IN2, LOW);
// Motor Wheel 3 - Left Back
analogWrite(motorW3_IN1, speed1PWM);
digitalWrite(motorW3_IN2, LOW);
// right side motors move in opposite direction
// Motor Wheel 4 - Right Front
digitalWrite(motorW4_IN1, LOW);
analogWrite(motorW4_IN2, speed1PWM);
// Motor Wheel 5 - Right Middle
digitalWrite(motorW5_IN1, LOW);
analogWrite(motorW5_IN2, speed1PWM);
// Motor Wheel 6 - Right Back
digitalWrite(motorW6_IN1, LOW);
analogWrite(motorW6_IN2, speed1PWM);
}
else if (ch6 > 1500) {
// Motor Wheel 1 - Left Front
digitalWrite(motorW1_IN1, LOW); // all wheels move at the same speed
analogWrite(motorW1_IN2, speed1PWM); // Forward
// Motor Wheel 2 - Left Middle
digitalWrite(motorW2_IN1, LOW);
analogWrite(motorW2_IN2, speed1PWM);
// Motor Wheel 3 - Left Back
digitalWrite(motorW3_IN1, LOW);
analogWrite(motorW3_IN2, speed1PWM);
// right side motors move in opposite direction
// Motor Wheel 4 - Right Front
analogWrite(motorW4_IN1, speed1PWM);
digitalWrite(motorW4_IN2, LOW);
// Motor Wheel 5 - Right Middle
analogWrite(motorW5_IN1, speed1PWM);
digitalWrite(motorW5_IN2, LOW);
// Motor Wheel 6 - Right Back
analogWrite(motorW6_IN1, speed1PWM);
digitalWrite(motorW6_IN2, LOW);
}
}
// Monitor the battery voltage
int sensorValue = analogRead(A0);
float voltage = sensorValue * (5.00 / 1023.00) * 3.02; // Convert the reading values from 5v to suitable 12V
// Send battery voltage value to transmitter
IBusSensor.loop();
IBusSensor.setSensorMeasurement(1, voltage * 100);
}
void calculateMotorsSpeed() {
// if no steering, all wheels speed is the same - straight move
if (ch0 > 1485 && ch0 < 1515) {
speed1 = speed2 = speed3 = s;
}
// when steering, wheels speed depend on the turning radius value
else {
// Outer wheels, furthest wheels from turning point, have max speed
// Due to the rover geometry, all three outer wheels should rotate almost with the same speed. They differe only 1% so we asume they are the same.
speed1 = s;
// Inner front and back wheels are closer to the turing point and have lower speeds compared to the outer speeds
speed2 = s * sqrt(pow(d3, 2) + pow((r - d1), 2)) / (r + d4);
// Inner middle wheel is closest to the turning point, has the lowest speed
speed3 = s * (r - d4) / (r + d4);
}
// speed value from 0 to 100% to PWM value from 0 to 255
speed1PWM = map(round(speed1), 0, 100, 0, 255);
speed2PWM = map(round(speed2), 0, 100, 0, 255);
speed3PWM = map(round(speed3), 0, 100, 0, 255);
}
void calculateServoAngle() {
// Calculate the angle for each servo for the input turning radius "r"
thetaInnerFront = round((atan((d3 / (r + d1)))) * 180 / PI);
thetaInnerBack = round((atan((d2 / (r + d1)))) * 180 / PI);
thetaOuterFront = round((atan((d3 / (r - d1)))) * 180 / PI);
thetaOuterBack = round((atan((d2 / (r - d1)))) * 180 / PI);
}