Tuesday, April 5, 2011

2.007 HW4 -- arcade drive, RC joystick mixing -- code should be free

#include
#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