servoWriter.ino

#include <Servo.h>
#include <SoftwareSerial.h>
SoftwareSerial mySerial(12,13); // RX, TX Sin-> 0 , Sout-> 12
SoftwareSerial mySerial2(7,8);
// Throttle test

#define LOW_THROTTLE 1104
#define HIGH_THROTTLE 1904
#define IDLE_THROTTLE 1300
#define NEUTRAL_THROTTLE 1500

Servo myThrottle;
Servo myRudder;
Servo myAile;
Servo myElevation;

int CUTOFF_PIN = 6;
int THROTTLE_PIN = 2;
int i = 0;
int flag = 0;
int flag2 = 0;
int done = 0;
int test,temp1,temp2,temp3,temp4, final;

void setup() {
  
  

  
  pinMode(CUTOFF_PIN, INPUT);
  Serial.begin(9600);
  
  myThrottle.attach(THROTTLE_PIN);
  myRudder.attach(3);
  myAile.attach(4);
  myElevation.attach(5);
  
   mySerial2.begin(9600);
   mySerial.begin(9600);
    
   mySerial2.print('u');
   delay(12);
   mySerial2.print('l');
  
  delay(10000);
  
  myThrottle.writeMicroseconds(mapCondense(NEUTRAL_THROTTLE));
  myAile.writeMicroseconds(mapCondense(NEUTRAL_THROTTLE));
  myElevation.writeMicroseconds(mapCondense(NEUTRAL_THROTTLE));
  myRudder.writeMicroseconds(mapCondense(NEUTRAL_THROTTLE));

  arm(); 
}

void arm() {
  
  myThrottle.writeMicroseconds(mapCondense(LOW_THROTTLE));
  myAile.writeMicroseconds(mapCondense(LOW_THROTTLE));
  myElevation.writeMicroseconds(mapCondense(LOW_THROTTLE));
  myRudder.writeMicroseconds(mapCondense(HIGH_THROTTLE));
  
  delay(3000);
  
  myThrottle.writeMicroseconds(mapCondense(IDLE_THROTTLE));
  myAile.writeMicroseconds(mapCondense(NEUTRAL_THROTTLE));
  myRudder.writeMicroseconds(mapCondense(NEUTRAL_THROTTLE));
  myElevation.writeMicroseconds(mapCondense(NEUTRAL_THROTTLE));
}

void laserInput(int distance){
  if ((distance < 1000)&&(flag2==0)){
    ConstantPwm(Throttle, 13);
    ConstantPwm (Rudder, 7); // works with laser
    ConstantPwm (Elevation, 7); // works with laser
    OCR0B = 12;
  }
  else {
    flag2 = 1;
    
    ConstantPwm(Throttle,11);
   delay(500);
    ConstantPwm(Throttle,6);
   delay(500);
   ConstantPwm(Aile,0);
    ConstantPwm(Elevation,0);
    ConstantPwm(Rudder,0);
    ConstantPwm(Throttle,0);
    done = 1;
    
  }
  
}

int mapCondense(int value) {
  
   return map(value, LOW_THROTTLE, HIGH_THROTTLE, 1000, 2000);
}

void loop() {
  
  if (digitalRead(CUTOFF_PIN) == HIGH) {
    
    myThrottle.writeMicroseconds(mapCondense(LOW_THROTTLE));
    myRudder.writeMicroseconds(mapCondense(NEUTRAL_THROTTLE));
    myAile.writeMicroseconds(mapCondense(NEUTRAL_THROTTLE));
    myElevation.writeMicroseconds(mapCondense(NEUTRAL_THROTTLE));
  }
  
  else {
    
    // loop code
  } 
  
  Serial.println(myThrottle.read());
  
   if (mySerial.available()){
     if (flag == 1) i = i+1;
         byte measurement = mySerial.read();
         test = measurement- '0';
         if ((test == 0 || test == 1 || test == 2 || test == 3 || test == 4 || test == 5 || test == 6 || test == 7 || test == 8 || test == 9)&& flag == 0) {
           i = 1;
         }
         switch (i){
           case 1:
             temp1  = measurement- '0';
             temp1 = temp1 * 1000;
             flag = 1;
             break;
            case 2:
             temp2  = measurement- '0';
             temp2 = temp2 * 100;
             flag = 1;
             break;
            case 3:
             temp3  = measurement- '0';
             temp3 = temp3 * 10;
             flag = 1;
             break;
            case 4:
              temp4  = measurement- '0';
              final = temp1 + temp2 + temp3 +temp4;
              break;
            case 5:
              if(flag==1){
               //  Serial.println(" ");
                // Serial.println(final, DEC);
                 flag = 0;
              }
              break;
            
            
         }
   
  if (done == 0) laserInput(final);


  }
}