void SetMotorSpeeds(signed int* left_p, signed int* right_p, signed long winkel)
{
signed int left = *left_p;
signed int right = *right_p;
static signed int lastleft = 0;
static signed int lastright = 0;
// winkel += 8;
// zu hohe werte abfangen
if (left > 0xFF) left = 0xFF;
if (left < -0xFF) left = -0xFF;
if (right > 0xFF) right = 0xFF;
if (right < -0xFF) right = -0xFF;
// zu geringe werte abfangen
//if (left <= LOW_SPEED_MAX && left >= LOW_SPEED_MIN) left = LOW_SPEED_MAX;
//if (left >= -LOW_SPEED_MAX && left <= -LOW_SPEED_MIN) left = -LOW_SPEED_MAX;
//if (right <= LOW_SPEED_MAX && right >= LOW_SPEED_MIN) right = LOW_SPEED_MAX;
//if (right >= -LOW_SPEED_MAX && right <= -LOW_SPEED_MIN) right = -LOW_SPEED_MAX;
// lienare steigerungsrate, um PID-Glied auf Korrektheit zu testen
// maximal speed um MAX_ACCELERATION aendern
if (MAX_ACCELERATION < abs(left - lastleft))
{
if (left > lastleft) left = lastleft + MAX_ACCELERATION;
else left = lastleft - MAX_ACCELERATION;
}
if (MAX_ACCELERATION < abs(right - lastright))
{
if (right > lastright) right = lastright + MAX_ACCELERATION;
else right = lastright - MAX_ACCELERATION;
}
// megatron auslesen alle 10 cycles
megatroncounter = (megatroncounter + 1);
if (megatroncounter == 10)
{
megatronleft = ReadMegatronLeft();
megatronright = ReadMegatronRight();
megatroncounter = 0;
}
// muss evtl angepasst werden
// verhaeltnis der megatrons errechnen
if (lastleft != 0) optimusprimeleft = megatronleft * 0x10 / abs(lastleft);
else optimusprimeleft = 0;
if (lastright != 0) optimusprimeright = megatronright * 0x10 / abs(lastright);
else optimusprimeright = 0;
// vor nachjustierung errechnete werte speichern
lastleft = left;
lastright = right;
if (optimusprimeleft > 0 && optimusprimeright > 0)
{
if (optimusprimeleft > optimusprimeright) // links "schneller" als rechts
{
// rechts schneller drehen
right = right * ((signed int) optimusprimeleft) / ((signed int) optimusprimeright);
}
if (optimusprimeright > optimusprimeleft) // rechts "schneller" als links
{
// links schneller drehen
left = left * ((signed int) optimusprimeright) / ((signed int) optimusprimeleft);
}
}
if (left < 0)
SetMotorSpeedLeft(MOTOR_BACKWARD, abs(left));
else
SetMotorSpeedLeft(MOTOR_FORWARD, left); // kein abs noetig, da left>=0
if (right < 0)
SetMotorSpeedRight(MOTOR_BACKWARD, abs(right));
else
SetMotorSpeedRight(MOTOR_FORWARD, right); // kein abs noetig, da right>=0
*left_p = left;
*right_p = right;
}
void Update() {
SetMotorSpeedLeft(GetLeftDriveMotorSpeed());
SetMotorSpeedRight(GetRightDriveMotorSpeed());
}