Ok so after some more testing this morning, I found that the 'Tone' function, seemingly doesn't like the new library for controlling the servo. Yesterday I tested it and it seemed to stop twitching and I was satisfied that after some work, this would be my solution.
This morning I tried to get the Bluetooth connection working and controlling the head movement and I found that what is actually happening, is that as soon as a tone is played, it seems to crash the entire servo library somehow and the is no longer any function on the servo at all. The twitching seemed to stop in this case because the library had crashed (or something).
Anyway, when I removed the Tone code from the start of the code, sure enough I can now control the head with the Bluetooth connection but the twitching is behaving as it was before. So, I have gone through the code again and I can't figure out what is causing the problem. I have attached the revised code in case someone has the time to look through it and see what I can't.
Thanks again guys, appreciate the time!
#include <ServoTimer2.h>
#include <Wire.h>
// ===== MOTOR CONTROL PINS =====
// Right Hand Motor pins of W-3
int enA = 6; //Pins 9 and 10 cannot be used as PWM pins when using the servo library
int in1 = 8; //HIGH for backward motion
int in2 = 7; //HIGH for forward motion
// Left Hand Motor pins of W-3
int enB = 3;
int in3 = 5; //HIGH for backward motion
int in4 = 4; //HIGH for forward motion
// motor triggers
byte LeftMotor = 0;
byte RightMotor = 0;
// ===== MPU5060 Setup =====
const int MPU = 0x68; // MPU6050 I2C address
float GyroX, GyroY, GyroZ;
float vehicleYaw;
// ===== LOOP TIMERS =====
float elapsedTime, currentTime, previousTime, currentMicros, dt;
// ===== Activity LED Timer stuff ====
unsigned long ActivityLEDInteval = 1500;
unsigned long blinkDuration = 400;
byte ActivityLEDState = HIGH;
unsigned long previousActivityLEDTime = 0;
int counter = 0;
// ===== PINOUT FOR UPPER HULL =====
// pitch servo removed for testing
//const byte PitchServo = A2; //Blue = Yaw servo = A1
const byte YawServo = A1; //Orange = Pitch Servo = A2
//A3 //Red = spare
//A4 //Brown = spare
const byte Speaker = A0; //Speaker RED = A0
//Green = Gnd (to 5v board)
//Yellow = Vcc (to 5v board)
// ===== PINOUT FOR LOWER HULL =====
const byte PowerLED = 10; //Green Power LED = 13
const byte ActivityLED = 11; //Orange Activity LED = 12
// ===== SERVO LIMITS =====
const byte maxYaw = 2100; // Yaw full left is 180
const byte minYaw = 750; // Yaw Servo full right is 10
// Yaw mid point is 1425
//Pitch servo end point will have to be look at again
//const byte maxPitch = 115; // Pitch Servo full down is 115
//const byte minPitch = 50; // Pitch Servo full up is 50
// Pitch Servo mid point is 80
// ===== Bluetooth Data =====
char BluetoothData;
float A;
float B;
byte connectionFound = 0;
// ===== HEAD MOTION =====
int headYawTarget;
byte headYaw = 4;
int yawPrevious;
ServoTimer2 Yaw;
//ServoTimer2 Pitch;
void setup() {
// Set all the motor control pins to outputs
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
// Turn off motors - Initial state
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
// Pinmodes
pinMode(PowerLED, OUTPUT);
pinMode(ActivityLED, OUTPUT);
pinMode(Speaker, OUTPUT);
//pinMode(PitchServo, OUTPUT);
pinMode(YawServo, OUTPUT);
//wire library setup for mpu5060
Serial.begin(9600);
Wire.begin(); // Initialize comunication
Wire.beginTransmission(MPU); // Start communication with MPU6050 // MPU=0x68
Wire.write(0x6B); // Talk to the register 6B
Wire.write(0x00); // Make reset - place a 0 into the 6B register
Wire.endTransmission(true); //end the transmission
// Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
Wire.beginTransmission(MPU);
Wire.write(0x1B); // Talk to the GYRO_CONFIG register (1B hex)
Wire.write(0x10); // Set the register bits as 00010000 (1000deg/s full scale)
Wire.endTransmission(true);
setupComplete();
}
// =======================================================================
// =======================================================================
// =======================================================================
void setupComplete()
{
// Reset servo positions and beep twice to indicate setup is complete
Yaw.attach(YawServo);
//Pitch.attach(PitchServo);
//Pitch.write(80);
Yaw.write(1425);
delay(450);
digitalWrite(PowerLED, HIGH); // power led on to indicate power
//tone(Speaker, 600, 500); // POST complete beep
}
// =======================================================================
// =======================================================================
// =======================================================================
void loop() {
previousTime = currentTime; // Previous time is stored before the actual time read
currentTime = millis(); // Current time actual time read
currentMicros = micros();
elapsedTime = (currentTime - previousTime) / 250; // not sure why it's by 250 but it works???
//get the input and sensor data first
btInput();
readYaw();
//move motors and servos
analogControlRight();
analogControlLeft();
//Effects
ActivityLEDChange();
}
// =======================================================================
// =======================================================================
// =======================================================================
void btInput()
{
if (Serial.available())
{
BluetoothData = Serial.read(); //Get next character from bluetooth
//Test for a connection
if (connectionFound == 0) //nake sure this only happens once
{
connectionFound = 1;
//tone(Speaker, 800, 100); // Connection Found
}
if (BluetoothData == 'A')
{
A = Serial.parseInt();
RightMotor = 1;
}
if (BluetoothData == 'B')
{
B = Serial.parseInt();
LeftMotor = 1;
}
if (BluetoothData == 'Y')
{
headYawTarget = Serial.parseInt();
yawPrevious = Yaw.read();
Yaw.write((yawPrevious * 0.4) + (headYawTarget * 0.6));
headYaw = 0;
}
}
}
// =======================================================================
// =======================================================================
// =======================================================================
void analogControlRight()
{
//measure the input from the controller and adjust the speed of the
//right wheel accordingly
if (RightMotor == 1)
{
if (A < 120 && A > -120) // dead zone stop all
{
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
A = 0;
}
else
{
if (A > 0) // forward so set motor direction
{
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
}
else // backward instead
{
A = -A;
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
}
}
analogWrite(enA, A);
RightMotor = 0;
}
}
// =======================================================================
// =======================================================================
// =======================================================================
void analogControlLeft()
{
//measure the input from the controller and adjust the speed of the
//left wheel accordingly
if (LeftMotor == 1)
{
if (B < 120 && B > -120) // dead zone - stop all
{
digitalWrite(in4, LOW);
digitalWrite(in3, LOW);
B = 0;
}
else
{
if (B > 0)
{
digitalWrite(in4, LOW);
digitalWrite(in3, HIGH);
}
else
{
B = -B;
digitalWrite(in4, HIGH);
digitalWrite(in3, LOW);
}
}
analogWrite(enB, B);
LeftMotor = 0; // reset the loop for the next value
}
}
// =======================================================================
// =======================================================================
// =======================================================================
void headYawMotion()
{
if (headYaw == 0) // turned on
{
dt += currentTime - previousTime;
if (dt >= 100) // wait for each servo update
{
Yaw.write(headYawTarget);
dt = 0;
headYaw = 4;
}
}
}
// =======================================================================
// =======================================================================
// =======================================================================
void readYaw()
{
Wire.beginTransmission(MPU);
Wire.write(0x43); // Gyro data first register address 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
GyroX = (Wire.read() << 8 | Wire.read()) / 131.0; // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
GyroY = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = GyroZ - 0.133;
vehicleYaw = vehicleYaw + GyroZ * elapsedTime;
}
// =======================================================================
// =======================================================================
// =======================================================================
void ActivityLEDChange() {
//Blink without delay - Thanks to Robin2 from the arduino forums
if (ActivityLEDState == LOW) {
if (currentTime - previousActivityLEDTime >= ActivityLEDInteval) {
ActivityLEDState = HIGH;
previousActivityLEDTime += ActivityLEDInteval;
}
}
else {
if (currentTime - previousActivityLEDTime >= blinkDuration) {
ActivityLEDState = LOW;
previousActivityLEDTime += blinkDuration;
}
}
// Change the LED Time and inteval after a number of
// LED changes to make it blink more randomly
if (digitalRead(ActivityLED) != ActivityLEDState)
{
counter++;
if (counter == 10)
{
if (ActivityLEDInteval == 1500)
{
ActivityLEDInteval = 100;
blinkDuration = 100;
counter = 3;
}
else
{
ActivityLEDInteval = 1500;
blinkDuration = 400;
counter = 0;
}
}
}
digitalWrite(ActivityLED, ActivityLEDState);
}
// =======================================================================
// =======================================================================
// =======================================================================
