Drone Experimental V0.2

// ServoDecodeTest

#include <ServoDecode.h>
#include <Servo.h>
#include <Scheduler.h>


Servo myservo[3];

char * stateStrings[] = {
 "NOT_SYNCHED", "ACQUIRING", "READY", "in Failsafe"};

int pwm[] = {3, 5, 6, 9};
int val[3];
int realval[3];
bool startup = true;
int potval;

void setup()                    // run once, when the sketch starts
{
  //myservo.attach(6);
 Serial.begin(9600);
 pinMode(12,OUTPUT);
 pinMode(13,OUTPUT);
 ServoDecode.begin();
 ServoDecode.setFailsafe(3,1234); // set channel 3 failsafe pulse  width
 for (int i = 0; i < 4; i++) {
  myservo[i].attach(pwm[i]);
 }
  Scheduler.startLoop(servoLoop);
}

/*
void loop()                     //use for testing and hopefully ruNning?
{
 int pulsewidth;

 // print the decoder state
 if( ServoDecode.getState()!= READY_state) {
   Serial.print("The decoder is ");
   Serial.println(stateStrings[ServoDecode.getState()]);
   for ( int i =0; i <=MAX_CHANNELS; i++ ){ // print the status of the first four channels
     Serial.print("Cx"); // if you see this, the decoder does not have a valid signal
     Serial.print(i);
     Serial.print("= ");
     pulsewidth = ServoDecode.GetChannelPulseWidth(i);
     Serial.print(pulsewidth);
     Serial.print("  ");
   }
   Serial.println("");
 }
 else {
   // decoder is ready, print the channel pulse widths
   for ( int i =1; i <=MAX_CHANNELS; i++ ){ // print the status of the first four channels
     Serial.print("Ch");
     Serial.print(i);
     Serial.print("= ");
     pulsewidth = ServoDecode.GetChannelPulseWidth(i);
     Serial.print(pulsewidth);
     Serial.print("  ");
   }
   Serial.println("");
   digitalWrite(12,LOW);
   digitalWrite(13,LOW);
 }
 delay(100); // update 2 times a second
}
*/

void loop() //use when actually using drone
{

/*
  while (startup) {
    potval = map(analogRead(A0), 0, 1023, 0, 155);
    Serial.println(potval);
    myservo.write(potval);
    delay(15);
    if (potval > 150) {
      startup = false;
    }
  }
  for (int i = 1; i <=MAX_CHANNELS; i++) {
    Serial.print("Ch");
    Serial.print(i);
    Serial.print("= ");
    pulsewidth = ServoDecode.GetChannelPulseWidth(i);
    Serial.print(pulsewidth);
    Serial.print(" ");
  }
*/

  for (int i = 0; i < 4; i++) {
    val[i] = map(ServoDecode.GetChannelPulseWidth(3), 1513, 1099, 0, 155);
  }
  yield();
}

void servoLoop(){
  for(int i=0; i<4; i++){
    if (val[i] > 50) {
      Serial.print(val[i]);
      Serial.print(" ");
      Serial.print("SPIN");
    }
  }
  for (int i = 0; i < 4; i++) {
    myservo[i].write(val[i]);
  }
  yield();
}

/*
void loop() {
  potval = analogRead(A0);            // reads the value of the potentiometer (value between 0 and 1023)
  potval = map(potval, 0, 1023, 0, 155);     // scale it to use it with the servo (value between 0 and 180)
  Serial.println(potval);
  myservo.write(potval);                  // sets the servo position according to the scaled value
  delay(15);                           // waits for the servo to get there
}
*/