/*
To do list:

-Get robot to go straight ahead. 

-Make values smooth for low speed control .

-How to make the servos stop totally and not slowly turn when stopped.

-Use pot for trim of centre position
*/



#include <Servo.h>

Servo servoone;
Servo servotwo; // create servo object to control a servo 
 

int chone;    //channel one left stick 
int chtwo;    //channel two righ stick 

int choner;   //Revers of channel one,might be used to store value of reversed servo so that robot can go strait
int chtwor;

void setup() 
{ 
  servoone.attach(9);  // attaches the servo on pin 9 and 10 to the servo object 
  servotwo.attach(10);
  
  pinMode(5,INPUT);
  pinMode(6,INPUT);
  
  Serial.begin(57600);
  Serial.println("Done with setup");
} 
 


void loop() 
{ 
  
  chone = pulseIn(5, HIGH, 100000);               //Get position of stick
  chone = map(chone, 1000, 2000, 0, 179);         //Map position to values used by servo 
  
  
  
  chtwo = (pulseIn(6, HIGH, 100000));
  chtwo = map(chtwo, 1000, 2000, 0, 179);
  

 
 
 if (( chone < 100) || ( 115 < chone ))    //Used to make the robot turn
 {
  //choner = 108 - chone
  servoone.write(chone);                 // both servos copy channel one (left stick) 
  servotwo.write(chone); 
 }




else                                     //When the stick is at rest
 {
  servoone.write(80);                    // both servos to center or stop
  servotwo.write(73); 
 }

 



  Serial.print(chone);                  // print values of channel one
  Serial.print("\t");
  Serial.println(chtwo);                // print values of channel two
  
  delay(15);                            // waits for the servo to get there 
}