Code: void DriveMecanumCartesian( DRIVE &drive, int x, int y, int rot ) { if (drive.flags & DRIVEF_FOUR_MOTORS) { int mag, maxMag, i; drive.motorPowers[IDX_FRONT_LEFT] = x + y + rot; drive.motorPowers[IDX_FRONT_RIGHT] = -x + y - rot; drive.motorPowers[IDX_REAR_LEFT] = -x + y + rot; drive.motorPowers[IDX_REAR_RIGHT] = x + y - rot; // // Normalize // maxMag = abs(drive.motorPowers[0]); for (i = 1; i < MAX_NUM_MOTORS; i++) { mag = abs(drive.motorPowers[i]); if (mag > maxMag) { maxMag = mag; } }
if (maxMag > MOTOR_MAX_VALUE) { for (i = 0; i < MAX_NUM_MOTORS; i++) { drive.motorPowers[i] = (drive.motorPowers[i]*MOTOR_MAX_VALUE)/maxMag; } } } else { // // Mecanum drive is only possible with 4 motors. For 2 motors, we // do arcade drive instead. // DriveArcade(drive, y, rot); } drive.flags |= DRIVEF_ON; } |