Posted on Leave a comment

SNARM with Joystick arduino code

SoftwareSerial mySerial(12, 13); // RX, TX

const int SERVOS = 4;

int PIN[SERVOS], value[SERVOS], currentAngle[SERVOS], MIN[SERVOS], MAX[SERVOS], INITANGLE[SERVOS];

Servo myservo[SERVOS];

int afrom[] = {90, 35, 135}, ato[] = {20, 140, 110}, amiddle[] = {20, 35, 135}, afinal[] = {179,75,95}, aafterfinal[] = {179,35,135};

int DELAYTIME = 200;

int servo_moving[SERVOS] = {0,0,0,0};

boolean stringComplete = false;

int bt_servo = 0;

int bt_move = 0;

int idle = 0;



void setup() {

  Serial.begin(9600);

  pinMode(3, OUTPUT);  

  digitalWrite(3, HIGH); 

  mySerial.begin(9600);
 

  init_Pins();
 

  //auto_mode();

}

void loop() {

  move_bt();

  move_joy();  

}

void init_Pins(){

  PIN[0] = 11;

  MIN[0] = 0;

  MAX[0] = 179;

  INITANGLE[0] = 90;

  PIN[1] = 10;

  MIN[1] = 35;

  MAX[1] = 179;

  INITANGLE[1] = 90;

  PIN[2] = 9;

  MIN[2] = 90;

  MAX[2] = 179;

  INITANGLE[2] = 155;

  PIN[3] = 5;

  MIN[3] = 0;

  MAX[3] = 179;

  INITANGLE[3] = 25;
 

  for (int i = 0; i < SERVOS; i++){

    myservo[i].attach(PIN[i]);

    myservo[i].write(INITANGLE[i]);

    value[i] = 0;

    idle = 0;

  }  

}

void move_bt(){ 

  checkSoftSerial();

  for (int i = 0; i < SERVOS; i++){

    currentAngle[i] = myservo[i].read();
 

    if (servo_moving[i] != 0){

      currentAngle[i] += servo_moving[i];

      currentAngle[i] = currentAngle[i] > MAX[i] ? --currentAngle[i] : currentAngle[i];

      currentAngle[i] = currentAngle[i] < MIN[i] ? ++currentAngle[i] : currentAngle[i];

      myservo[i].write(currentAngle[i]);

      delay(20);

    }

  }

}

void checkSoftSerial() {

  String str = "";
 

  if (mySerial.available()){

    for (int i = 0 ; i < 2; i++){

      str += (char)mySerial.read(); 

    }

    // the servo to move

    int value = str.toInt();

    bt_servo = value / 10;
 

    // the direction to move

    int angle = value % 10;

    if (angle == 2) bt_move = 1;

    else if (angle == 1) bt_move = -1;

    else bt_move = 0;
 

    servo_moving[bt_servo] = bt_move;

  }

}

void move_joy(){

  for (int i = 0; i < SERVOS; i++){

    value[i] = analogRead(i);

    currentAngle[i] = myservo[i].read();
 
 

    if (value[i] > 612) {

      idle = 0;      

      if (currentAngle[i] > MIN[i]) --currentAngle[i]; 

    } else if (value[i] < 412) {

      idle = 0;

      if (currentAngle[i] < MAX[i]) ++currentAngle[i]; 

    } else {

      ++idle;

    }
 

    if (idle == 100){

      myservo[i].detach();

    }

  }      
 

  for (int i = 0 ; i < SERVOS; i++){

    if (!myservo[i].attached()) myservo[i].attach(PIN[i]);

    myservo[i].write(currentAngle[i]);

  }  

  delay(20);

}

void auto_mode(){

  for (int i = 0; i < 2; i++){

    closeclaw(false);

    armfromto(afrom, ato);

    closeclaw(true);

    delay(DELAYTIME); 

    armfromto(ato, amiddle);

    delay(DELAYTIME);   

    armfromto(amiddle, afinal);

    closeclaw(false);

    delay(DELAYTIME);

    armfromto(afinal, aafterfinal);

    delay(DELAYTIME);

    armfromto(aafterfinal, afrom);

    delay(DELAYTIME);

  } 

}

void armfromto(int *arrf, int *arrt){

  int lp[3], seg = 3, sign;
 

  for (int i = 0; i < 3; i++){    

    lp[i] = abs((arrt[i] - arrf[i])/seg);

  }
 

  //delay(DALAYTIME);

  for (int i = 0; i < 3; i++){

    sign = arrt[i] - arrf[i] > 0 ? 1 : -1;

    for (int j = 0; j < lp[i]; j++){

      myservo[i].write(arrf[i]+j*seg*sign);

      delay(20);

    }

    delay(DELAYTIME);

  }

}

void closeclaw(boolean op){

  if (op){

    myservo[3].write(5);

  } else {

    myservo[3].write(30);

  }

}
Leave a Reply

Your email address will not be published. Required fields are marked *