// Get Distance driven from encoders double DriveTrain::GetDistance() { // 25.2627908 is ticks per inch driven // (360 * 4 / 3) / (6 * PI) = 25.2627908 int enc = GetLeftEncoder(); if (enc < 0) return (double)-enc / TICKS_PER_INCH; else return (double)enc / TICKS_PER_INCH; }
void sendCommMessage(void){ if(EnableSensorFeedbackMessages){ UARTprintf("(: %d %d %d %d %d%.3d %d :)\r\n", GetLeftEncoder(), GetRightEncoder(), GetV(), GetW(), GetTime(),GetMS(), GetMessageRate() ); //UARTprintf("^.- ROLL %d PTCH %d YAWW %d -.^\r\n", roll, pitch, yaw ); /*UARTprintf("(: %d %d %d %d %d %d %d %d%.3d %d :)\r\n", GetLeftEncoder(), GetRightEncoder(), GetV(), GetW(), GetRoll(), GetPitch(), GetYaw(), GetTime(), GetMS(), GetMessageRate() );*/ } }