Here's the code i'm using on the arduino for the joystick control
#include <Servo.h>
int deadzone = 15;
int rev1 = 2;
int rev2 = 3;
int rev3 = 4;
int rev4 = 5;
int joyPin1 = 0; // slider variable connecetd to analog pin 0
int joyPin2 = 1; // slider variable connecetd to analog pin 1
int joyPin3 = 2;
int servospd1 = 1;
int servospd2 = 1;
int servospd3 = 1;
int servospd4 = 1;
int axis1[4] = {0, 600 , 755 , 970}; //y-axis {value, min, center, max}
int axis2[4] = {0, 590 , 780 , 1023}; //x-axis
int axis3[3] = {0, 400, 700};
Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
void setup()
{
pinMode(rev1, OUTPUT); // initializes digital pins 0 to 7 as outputs
pinMode(rev2, OUTPUT);
pinMode(rev3, OUTPUT);
pinMode(rev4, OUTPUT);
myservo1.attach(12);
myservo2.attach(11);
myservo3.attach(10);
myservo4.attach(9);
Serial.begin(9600);
}
int forward(int data) {
return ( ( data - 190 ) / 4.1111111111 );
}
int reverse( int data ){
return ( 180 - ( data - 90 ) / .2222222222222 );
}
void loop()
{
// reads the value of the variable resistor
//axis1 = treatValue( analogRead( joyPin1 ) );
myservo1.write( abs(servospd1 - servospd2) );
myservo2.write( abs(servospd1 + servospd2) );
myservo3.write( servospd3 );
//myservo4.write( servospd4 );
//////LEFT MOTOR /////
if( axis1[0] >= (deadzone) )
{
servospd1 = ( float(axis1[0]) * 180 / (axis1[3]-axis1[2]) );\
}
else if( axis1[0] <= (-deadzone) )
{
servospd1 = ( float(axis1[0]) * 180 / (axis1[2]-axis1[1]) );
}
else
{
servospd1 = 0;
}
/////RIGHT MOTOR/////
if( axis2[0] >= (deadzone) )
{
servospd2 = ( float(axis2[0]) * 180 / (axis2[3]-axis2[2]) );
}
else if( axis2[0] <= (-deadzone) )
{
servospd2 = ( float(axis2[0]) * 180 / (axis2[2]-axis2[1]) );
}
else
{
servospd2 = 0;
}
///////VERTICAL MOTOR//////
if( axis3[0] > 1024 )
{
servospd3 = 90;
digitalWrite(rev3, LOW);
}
else if( 850 < axis3[0] && axis3[0] < 875)
{
servospd3 = 90;
digitalWrite(rev3, HIGH);
}
else if( 690 < axis3[0] && axis3[0] < 720 )
{
servospd3 = 0;
digitalWrite(rev3, LOW);
}
////////////
if ((servospd1 - servospd2) < 0)
{
digitalWrite(rev1, HIGH);
}
else
{
digitalWrite(rev1, LOW);
}
if ((servospd1+servospd2) < 0)
{
digitalWrite(rev2, HIGH);
}
else
{
digitalWrite(rev2, LOW);
}
for(int i=0; i<10; i++)
{
axis1[0] += analogRead( joyPin1 );
axis2[0] += analogRead( joyPin2 );
axis3[0] += analogRead( joyPin3 );
myservo1.write( abs(servospd1 - servospd2) );
myservo2.write( abs(servospd1 + servospd2) );
myservo3.write( servospd3 );
myservo4.write( servospd4 );
delay(7);
}
axis1[0] = ( float(axis1[0]) / 10 ) - axis1[2];
axis2[0] = ( float(axis2[0]) / 10 ) - axis2[2];
axis3[0] = ( float(axis3[0]) / 10 );
Serial.print( axis1[0] );
Serial.print( "\t" );
Serial.print( axis2[0] );
Serial.print( "\t" );
Serial.print( axis3[0] );
Serial.print( "\n" );
/*
Serial.print( servospd1 );
Serial.print( "\t" );
Serial.println( servospd2 );
*/
//delay(100);
} derp
No comments:
Post a Comment