Hey. Im running out of ideas as to why my program is not working. The program is pretty much a control system for a vehicle and has a gyro sensor for feedback. For some reason as I power up or down the motor. The gyro(analog) seems to spit out weired data. I am currently using the handshake method to aquire data. Regardless of what i do to filter out this information i am not having any luck. Is is possible that as i power up the motor that the voltage to the gyro is changning and therefore causing the analog signal to jump around. I am currently using the the 5v and 3v3 on the arduino duminelove board.
Arduino code:
//Libraries
#include <AFMotor.h>
#include <SoftwareSerial.h>
#include "math.h"
//Software Serial
#define rxPin 6 // Defines the "recieve" pin for software serial to motor controller
#define txPin 7 // Defines the "transmit" pin for software serial to motor controller
#define resetPin 8 // Defines the reset pin for the motor controller
//Variable Declaration
SoftwareSerial motorSerial = SoftwareSerial(rxPin, txPin);
float vzero=analogRead(0);
float vinputv=analogRead(1);
float vinputh=analogRead(2);
float biasv=0.00;
float biash=0.00;
float sense=2.00;
float angvv=0.000000;
float angvh=0.000000;
float analogv=0.00;
float analogh=0.00;
float angvvcorr=4.28*2.5;
float angvhcorr=4.28*2.5;
int correction=0;
int mspeed=0;
int input=0;
char upk='8';
char downk='2';
char stablek='5';
char leftk='4';
char rightk='6';
char sup='a';
char sdown='s';
int setspeed=0;
void setup()
{
Serial.begin(9600);
Serial.flush();
InitMotor();
}
void loop()
{
if ( Serial.available() > 0)
{
input=Serial.read();
if (input==sup)
{
setspeed=setspeed+5;
SetMotor(setspeed);
}
else if (input==sdown)
{
setspeed=setspeed-5;
SetMotor(setspeed);
}
else if(input==upk)
{
}
else if(input==stablek)
{
}
else if(input==downk)
{
}
else if(input==leftk)
{
}
else if(input==rightk)
{
}
else if(input=='q')
{
gyro();
}
}
}
void gyro()
{
vzero=analogRead(0);
vinputv=analogRead(1);
vinputh=analogRead(2);
if (correction==0)
{
biasv=vinputv-vzero;
biash=vinputh-vzero;
correction=1;
}
analogv=(vinputv)-(vzero)-biasv;
analogh=(vinputh)-(vzero)-biash;
if((analogv==-2)||(analogv==-1)||(analogv==1)||(analogv==2))
{
analogv=0.00;
}
if((analogh==-2)||(analogh==-1)||(analogh==1)||(analogh==2))
{
analogh=0.00;
}
angvv=(angvvcorr*(analogv/sense));
angvh=(angvhcorr*(analogh/sense));
Serial.write(char(int(angvv)/3));
Serial.write(char(int(angvh)/3));
}
void InitMotor()
{
pinMode(txPin, OUTPUT);// Serial Transmit Pin
pinMode(resetPin, OUTPUT); // Motor Reset Pin
motorSerial.begin(9600);
// Reset the motor controller (By setting low then high signals to the resetPin)
digitalWrite(resetPin, HIGH);
delay(10);
digitalWrite(resetPin, LOW);
delay(10);
digitalWrite(resetPin, HIGH);
delay(10);
}
void SetMotor(int speed)
{
int motorIndex=0;
unsigned char buffer[4];// Create the packet buffer
unsigned char index = 0;
unsigned char targetSpeed = speed;
buffer[0] = 0x80;
buffer[1] = 0x00;
buffer[2] = 0x00 | index;
buffer[3] = targetSpeed;
SendPacket(buffer, 4);
}
void SendPacket(unsigned char *buffer, int bufferCount)
{
for(int i = 0; i < bufferCount; i++)
{
Serial.write('t');
motorSerial.print(char(buffer[i]));
Serial.write('t');
}
}
processing code( has images so u wont be able to run it):
import processing.serial.*;
import controlP5.*;
ControlP5 controlP5;
Serial myPort;
int myColorBackground = color(200,200,200);
//Layout
int sw;
int sh;
//Image Call Up
PImage arrowr; // Declare variable "a" of type PImage
PImage arrowl; // Declare variable "a" of type PImage
PImage stable; // Declare variable "a" of type PImage
PImage arrowu; // Declare variable "a" of type PImage
PImage arrowd; // Declare variable "a" of type PImage
PImage warrowr; // Declare variable "a" of type PImage
PImage warrowl; // Declare variable "a" of type PImage
PImage wstable; // Declare variable "a" of type PImage
PImage warrowu; // Declare variable "a" of type PImage
PImage warrowd; // Declare variable "a" of type PImage
PImage angvh;
PImage angvv;
//Keys
char upk='8';
char downk='2';
char leftk='4';
char rightk='6';
char stablek='5';
int test=0;
void setup()
{
sw=screen.width-100;
sh=screen.height-100;
size(sw,sh);
//Unpressed Control Images
arrowr = loadImage("arrowr.jpg"); // Load the image into the program
arrowl = loadImage("arrowl.jpg"); // Load the image into the program
stable = loadImage("stable.jpg"); // Load the image into the program
arrowu = loadImage("arrowu.jpg"); // Load the image into the program
arrowd = loadImage("arrowd.jpg"); // Load the image into the program
//Pressed Control Images
warrowu = loadImage("warrowu.jpg"); // Load the image into the program
warrowd = loadImage("warrowd.jpg"); // Load the image into the program
warrowl = loadImage("warrowl.jpg"); // Load the image into the program
warrowr = loadImage("warrowr.jpg"); // Load the image into the program
wstable = loadImage("wstable.jpg"); // Load the image into the program
//sensor data
angvh = loadImage("angvh.jpg"); // Load the image into the program
angvv = loadImage("angvv.jpg"); // Load the image into the program
int rangle=0;
//Connect and show available COM ports
myPort = new Serial(this, Serial.list()[1],9600);
// println(Serial.list());
//Slider Motor Control
controlP5 = new ControlP5(this);
Slider s = controlP5.addSlider("slider",0,127,0,450,575,10,100);
}
void draw()
{
background(myColorBackground);
image(arrowr, 0.672*sw, 0.748*sh);
image(arrowl, 0.597*sw, 0.75*sh);
image(stable, 0.6335*sw, 0.75*sh);
image(arrowu, 0.640*sw, 0.69*sh);
image(arrowd, 0.640*sw, 0.7988*sh);
image(angvh,0.8209*sw,0.1938*sh);
image(angvv,0.8932*sw,0.25*sh);
fill(0);
rect(420, 150, 550, 300);
if(keyPressed==true)
{
if (key=='a')
{
test=1;
control(key);
}
else if(key=='s')
{
test=1;
control(key);
}
else if(key==upk)
{
image(warrowd, 0.640*sw, 0.7988*sh);
image(wstable,0.6335*sw, 0.75*sh);
image(warrowl, 0.597*sw, 0.75*sh);
image(warrowr, 0.672*sw, 0.748*sh);
control(key);
}
else if(key==downk)
{
image(wstable, 0.6335*sw, 0.75*sh);
image(warrowl, 0.597*sw, 0.75*sh);
image(warrowr, 0.672*sw, 0.748*sh);
image(warrowu, 0.640*sw, 0.69*sh);
control(key);
}
else if(key==stablek)
{
image(warrowl, 0.597*sw, 0.75*sh);
image(warrowr, 0.672*sw, 0.748*sh);
image(warrowu, 0.640*sw, 0.69*sh);
image(warrowd, 0.640*sw, 0.7988*sh);
control(key);
}
else if(key==rightk)
{
image(warrowu, 0.640*sw, 0.69*sh);
image(warrowd, 0.640*sw, 0.7988*sh);
image(wstable, 0.6335*sw, 0.75*sh);
image(warrowl, 0.597*sw, 0.75*sh);
control(key);
}
else if(key==leftk)
{
image(warrowr, 0.672*sw, 0.748*sh);
image(warrowd, 0.640*sw, 0.7988*sh);
image(wstable, 0.6335*sw, 0.75*sh);
image(warrowu, 0.640*sw, 0.69*sh);
control(key);
}
}
if(test!=1)
{
gyro();
}
else
{
test=0;
}
}
int control(char micro)
{
myPort.write(micro);
//println(micro);
return(0);
}
void slider(int theColor)
{
// println(theColor);
myPort.write(char(theColor));
}
void gyro()
{
myPort.write('q');
if ((myPort.available()>0)&&(myPort.read()!='t'))
{
int angvv=0;
int angvh=0;
angvv=myPort.read();
angvh=myPort.read();
print("angvv:");
println(angvv);
print("angvh:");
println(angvh);
if(angvv==-1)
{
angvv=0;
print("wtf");
}
if(angvh==-1)
{
angvh=0;
print("wtf");
}
if(angvv==0)
{
ellipseMode(CENTER);
fill(0);
ellipse(1202, 160, 15, 15);
}
else if(angvv>125)
{
ellipseMode(CENTER);
fill(0);
ellipse(1202+75, 160, 15, 15);
}
else if(angvv<125)
{
ellipseMode(CENTER);
fill(0);
ellipse(1202-75, 160, 15, 15);
}
if(angvh==0)
{
ellipseMode(CENTER);
fill(0);
ellipse(1202, 298, 15, 15);
}
else if(angvh>125)
{
ellipseMode(CENTER);
fill(0);
ellipse(1202, 298+75, 15, 15);
}
else if(angvh<125)
{
ellipseMode(CENTER);
fill(0