Update servoWriter.ino

AutonomousNAZA/servoWriter.ino

@@ -1,5 +1,7 @@
 #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
@@ -14,9 +16,17 @@ 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);
 
@@ -25,6 +35,13 @@ void setup() {
   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));
@@ -50,6 +67,30 @@ void arm() {
   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);
@@ -71,4 +112,47 @@ void loop() {
   } 
 
   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);
+
+
+  }
 }