rmm81_code_snippets.txt

//Changes encoder values into pose:

double right_angle = 30.0 * M_PI/180.0, left_angle = 150.0 * M_PI/180.0, rear_angle = 90.0 * M_PI/180.0;
double bot_diameter = 11.0;

encoder_pose.x += ((ri_getWheelEncoder(&ri, RI_WHEEL_RIGHT) * cos(right_angle)) 
                 + (ri_getWheelEncoder(&ri, RI_WHEEL_LEFT) * cos(left_angle)));
				
encoder_pose.y += ((ri_getWheelEncoder(&ri, RI_WHEEL_RIGHT) * sin(right_angle)) 
                 + (ri_getWheelEncoder(&ri, RI_WHEEL_LEFT) * sin(left_angle)));

encoder_pose.theta = (ri_getWheelEncoderTotals(&ri, RI_WHEEL_REAR)/(M_PI * bot_diameter));