Monday, January 9, 2012

ROV joystick control

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