New Drivetrain Cde

import com.pi4j.io.gpio.*;
import com.pi4j.wiringpi.Gpio;
import com.pi4j.wiringpi.GpioUtil;

public class RobotControl {

    private static final Pin LEFT_FORWARD_PIN = RaspiPin.GPIO_00;
    private static final Pin LEFT_BACKWARD_PIN = RaspiPin.GPIO_01;
    private static final Pin RIGHT_FORWARD_PIN = RaspiPin.GPIO_02;
    private static final Pin RIGHT_BACKWARD_PIN = RaspiPin.GPIO_03;
    private static final int PWM_RANGE = 1000;
    private static final int PWM_FREQUENCY = 100;
    
    private static GpioController gpio;
    private static GpioPinPwmOutput leftForward;
    private static GpioPinPwmOutput leftBackward;
    private static GpioPinPwmOutput rightForward;
    private static GpioPinPwmOutput rightBackward;

    public static void initialize() {
        gpio = GpioFactory.getInstance();
        Gpio.pwmSetMode(Gpio.PWM_MODE_MS);
        Gpio.pwmSetRange(PWM_RANGE);
        Gpio.pwmSetClock(192);

        leftForward = gpio.provisionPwmOutputPin(LEFT_FORWARD_PIN);
        leftBackward = gpio.provisionPwmOutputPin(LEFT_BACKWARD_PIN);
        rightForward = gpio.provisionPwmOutputPin(RIGHT_FORWARD_PIN);
        rightBackward = gpio.provisionPwmOutputPin(RIGHT_BACKWARD_PIN);

        leftForward.setPwm(0);
        leftBackward.setPwm(0);
        rightForward.setPwm(0);
        rightBackward.setPwm(0);
    }

    public static void stop() {
        leftForward.setPwm(0);
        leftBackward.setPwm(0);
        rightForward.setPwm(0);
        rightBackward.setPwm(0);
    }

    public static void moveForward(int speed) {
        leftForward.setPwm(speed);
        rightForward.setPwm(speed);
    }

    public static void moveBackward(int speed) {
        leftBackward.setPwm(speed);
        rightBackward.setPwm(speed);
    }

    public static void turnLeft(int speed) {
        leftBackward.setPwm(speed);
        rightForward.setPwm(speed);
    }

    public static void turnRight(int speed) {
        leftForward.setPwm(speed);
        rightBackward.setPwm(speed);
    }

    public static void cleanup() {
        gpio.shutdown();
    }

    public static void main(String[] args) {
        initialize();

        // Move forward at speed 500 for 2 seconds
        moveForward(500);
        try {
            Thread.sleep(2000);
        } catch (InterruptedException e) {
            e.printStackTrace();
        }
        stop();

        cleanup();
    }
}