/*
 * BT-X21 PITCH CONTROL DUAL ARDUINO PROJECT
 * Based on RF 433MHz. Made by Mc Rae Mateo Badua(2017)based on
 * Syma Gimbal Control by Muhammad Imam Zulkarnaen (2017)
 */

#include <SoftwareServo.h>
#include <VirtualWire.h>

#define RX_PIN      12
#define GIMBAL_PIN  2

///// CHANGE HERE //////
#define MAX    130     //Max Angle
#define MIN    90      //Min Angle
#define SMOOTH 8       //1..10 OR 100 to disable
////////////////////////
SoftwareServo GIMBAL;

uint8_t packet[1], val, value;
float c_pos, t_pos, easing;

void setup() {
  pinMode(13,OUTPUT);
  vw_set_rx_pin(RX_PIN);
  vw_setup(1200);     
  vw_rx_start();
  GIMBAL.attach(GIMBAL_PIN);
  GIMBAL.write(MIN);
  //Serial.begin(9600);     
}

void loop(){  
  readPacket();
  val = map(value, 0, 255, MIN, MAX);
  gimbal(val);
  
}

/// READ PACKET FUNCTION
void readPacket(){
  uint8_t len = VW_MAX_MESSAGE_LEN;
  
  if(vw_get_message(packet, &len)){
    if(len==1){
      value = packet[0];
      digitalWrite(13, HIGH);
      //Serial.println(packet);
    }
  }
  delay(5);
  digitalWrite(13,LOW);
} 

/// WRITE GIMBAL FUNCTION
void gimbal(int pot){
   float diff;
  
   easing = (float)(SMOOTH * 10);
   easing /= 1000;
   t_pos = (float)pot;
   
   diff = t_pos - c_pos;
   if(diff != 0.00) c_pos += diff * easing;
  
   GIMBAL.write((int)c_pos);
   delay(5);
   SoftwareServo::refresh();
}