Drive motors have two outputs each: one high/low for direction, and one PWM for speed.The two encoders are simply Analog In. Code is already in place to count ticks.The gyroscope is analog in, but gives rate of degree change/second, but I already have running code to keep an absolute angle. Whatever heading the robot is when powered on is 0 degrees.
motorfunction(length in cm, heading in degrees)set both motors to half speedif given heading is less than current heading, set left motor to reverse, right motor to foward.else if set motor left motor to foward, right motor to reverse.set both motors to 0 speed when given heading == current heading.set current tick count for both encoders to 0convert given length in cm into tick count. set both motors to full speed, foward.if current tick count is less than set tick count if the current heading is larger than set heading set left motor slightly slower until heading is the same. if the current heading is less than the set heading set right motor slightly slower until heading is the same.stop when the average between the two encoder tick counts are >= given tick count.
if destination_heading is less than current_heading destination_heading += 360;If destination_heading - current_heading < 180 turn clockwise.else turn counterclockwise.