#define RC1 8
#define RC2 9
Servo Lservo;
Servo Rservo;
void setup()
{ digitalWrite(RC1,LOW);
digitalWrite(RC2,LOW);
pinMode(RC1, INPUT);
pinMode(RC2, INPUT);
Lservo.attach(6);
Rservo.attach(7);
Serial.begin(9600);
}
void loop()
{
/* 0. Read in RC, constrain to make sure values between 1000 and 2000
1. map ch1 (left/right), x_axis, to -+ 500
1b. SKIPPED - check if it's in the deadband
2. add to y axis values for left, subtract for right
3. constrain result to 1000,2000
4. map to servo values -> 0 fwd, 90 neutral, 180 backward (left and right servo are mapped OPPOSITELY)
*/
int x_axis = constrain( pulseIn(RC1, HIGH, 25000), 1000,2000);
int y_axis= constrain( pulseIn(RC2, HIGH, 25000), 1000,2000);
// Serial.println(x_axis);
int x_offset = map(x_axis,1000,2000,-500,500);
int left = constrain(y_axis + x_offset, 1000, 2000);
int right = constrain(y_axis - x_offset, 1000, 2000);
Lservo.write( map(left,1000,2000,180,0) );
Rservo.write( map(right,1000,2000,0,180) );
}
No comments:
Post a Comment