diff --git a/README.md b/README.md index 8edaeba..a82b3f1 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,8 @@ > The **pigpio library** will also need to be installed. To do this, run the following command on your Raspberry Pi: > + `$sudo apt install pigpio python-pigpio python3-pigpio` > -> Once the project is cloned, you may open the **core/src** folder, then go to **Robot.java**. This is where you will focus to program your robot. +> You will also need to clone the RPiBotLib template repo in GitHub. You may find that here: https://github.com/Ghozti/RPiBotLibTemplate +> . Once the project is cloned, ensure that you have the latest version of RPiBotLib in your build.gradle file. You may now go to `core/src/ghozti/rpibotlib/template/Robot.java`. This will be the base of your robot program and where you will write all of your code. > > It is recommended to set up **VNC Viewer** onto your raspberry pi in order to be able to remote control it from your PC. > + visit: [How To Set Up VNC Connect on Raspberry Pi - RealVNC](https://www.realvnc.com/en/blog/how-to-setup-vnc-connect-raspberry-pi/) @@ -33,7 +34,7 @@ > `robotShutDown()` is similar `robotInit()` except it will only run when the driver station state is set to **"kill"**. By default, `robotShutDown()` will cancel any ongoing method and will suspend the program. > ### Understanding The Driver Station ->![DriverStation](https://cdn.discordapp.com/attachments/412278280180203552/1002756776724271195/Capture.PNG) +>![DriverStation](https://cdn.discordapp.com/attachments/1134223436856696902/1138852944440213514/Screenshot_2023-08-09_110809.png) > > The driver station has several features to it, including 4 functional buttons: > + **Teleop:** where the user is able to take control of the robot via a joystick or controller. @@ -47,7 +48,11 @@ > > When a sensor is added, the driver station will display its name as well as its returned value. > -> As stated before there is a third robot state known as kill, to set this mode simply press **k.** +> Next to the sensors, you will find the PID section. This will allow up to one PID controller at a time to be displayed and it will display the current error, sensorvalue,output, if the controller is on target and the current elapsed time.In that order +> +> As stated before there is a third robot state known as kill, to set this mode simply press **k** +> +> ***NOTE it is VERY important to suspend the program by using the kill function. DO NOT SIMPLY X OUT*** > > The current robot mode will always be indicated in **green.** > @@ -57,7 +62,7 @@ > > ### Context > The Context class is a PI4J class which is used to be able to control the raspberry pi GPIO. This class is mandatory to instantiate in order to be able to execute your program. The best place to instantiate this class is in `robotInit()`. No parameters are needed in order to instantiate this class. -> + For more inforation on the PI4J library, visit: [Welcome to Pi4J](https://pi4j.com/) +> + For more information on the PI4J library, visit: [Welcome to Pi4J](https://pi4j.com/) > > ### PwmConfig, DigitalOutputConfig, and DigitalInputConfig > These classes are wrapper classes of the Pi4J config classes used to declare a new pin for pwm or digital input/output use. These wrappers allow a simple use of the Pi4J classes. The wrapper classes contain a method that return a dedicated object of said configuration. The digital input/output method will require a Pi4J context object, a pin address, and a pin ID in the form of a string and a name. The pwm config method will require a context object, a pin address, and a pwm type whether it be digital or hardware. @@ -73,11 +78,36 @@ > this class is used to develop timed-based autonomous code. It is simple to use as you only need to instantiate it and use the `addCommand()` method to program your autonomous. Once done, simply call the `runAuto()` method in `autonomousPeriodic()`. > > ### TimedCommand -> This class is used to create new commands to add to the `TimedAutoBase` list of commands. The constructor takes a `DifferentialDrive` object as well as an x and y value and the duration in seconds (double) or milliseconds (long) for the command to be executed for. +> This class is used to create new commands to add to the `TimedAutoBase` list of commands. The constructor takes a `DifferentialDrive` object as well as an x and y value and the duration in seconds (double) or milliseconds (long) for the command to be executed for. + +### Advanced Programming +>### ServoHat +> As of version 1.1.1, RPiBotLib has added support to the raspberry pi servo hat: PCA9685. By integrating the diozero library to this lib. +> The hat can take up to 12 servos at a time. The class can be found in `ServoDriver`. This class has 2 constructors, +> `ServoDriver()` which takes to arguments and defaults the PCA9685 class arguments to `(I2Constants.CONTROLLER_1,0x40,50)`, where the controller ID will be set to 1, the I2C address will be set to 0x40 and the PWM frequency will be set to 50. The other constructor will allow you to manually set these arguments to a different value. +> #### Adding servo devices: +> To add a servo you can either call the `addServo(int gpio, ServoTrim trim)`method where `gpio` will be the ID of the port wich you added the servo on the hat and `servoTrim` will be the servo type. Or the `addAllServos(ServoTrim trim)` method where it will add all 12 servos of the same type at once. +>### PID +> In addition to servo drivers, version 1.1.1 also adds the support for PID controllers. +> The `PIDController` class implements the DisplayAble interface meaning that values can be displayed on the driver station for debbugging. +> The class takes 2 constructors `PIDController(double kP, double kI, double kD, double iLimit, double maxTolerance, double minTolerance, double target)`, where `kP`, `kI`,`kD`, will be your PID values, `iLimit` will be the threshold where `kI` gets applied to the formula, `maxTolerance` and `minTolerance` will be thresholds where the PID controller will be satisfied and `target` will be the set value for the PID controller to get to. +> ***Note: `maxTolerance` = `target`+ `maxTolerance` , `minTolerance` = `target` - `minTolerance`*** +>#### Using the PID controller +> The `getOutput(double sensorValue)` method will be used to get the output needed to reach the PID target +>this method automatically calculates the needed output based on the parameters provided. If you are using more than one PID controller, it is recommended to call the `pause()` function as it will minimize the chance of conflict between the two when one is not in use (or satisfied) +> +> Learn more about PID with these resources: +> https://gamzeyilan1.medium.com/pid-controller-for-absolute-beginners-4a49c58c8098#:~:text=PID%20is%20short%20for%20Proportional,how%20each%20control%20system%20works. +> https://www.youtube.com/watch?v=jIKBWO7ps0w > + +### More Examples +> You may find more examples in the lib Robot sample class here: +> https://github.com/Ghozti/RPiBotLib/blob/master/core/src/Robot/Robot.java + ### Running your project > ### Creating a jar -> Once you have finished programming your robot, you will need to create a jar for it. Doing this is simple, all you need to do is select the gradle icon on your IDE -> "Execute Gradle Task" -> "gradle: :desktop:dist". Once done you should find a jar file under "desktop/build/libs" in your project. +> Once you have finished programming your robot, you will need to create a jar for it. Doing this is simple, all you need to do is select the gradle icon on your IDE -> `Execute Gradle Task` ->`gradle: :desktop:dist`. Once done you should find a jar file under `desktop/build/libs` in your project. > > ### Running your jar > After you have created your jar all you have to do is run it on your raspberry pi. ***you will need to run this project as sudo in order to be able to initialize pigpio*** @@ -106,3 +136,5 @@ ### Version Logs >#### ***v1.0.0*** - provides a basic library with everything needed to run a simple robot with autonomous capabilities >#### ***v1.1.0*** - adds compatibility with PCA9685 devices +>#### ***v1.1.1*** - added servo driver and PID support +>#### ***v1.1.2*** - fixed no controller found issue when user is not using an xbox controller, optimized program performance, updated readMe driver station screenshot diff --git a/core/src/Robot/Robot.java b/core/src/Robot/Robot.java index 6a45485..dcaeae8 100644 --- a/core/src/Robot/Robot.java +++ b/core/src/Robot/Robot.java @@ -1,43 +1,129 @@ package Robot; +import com.pi4j.Pi4J; import com.pi4j.context.Context; +import com.pi4j.io.pwm.PwmType; +import pibotlib.graphics.DriverStation; +import pibotlib.lib.addons.DigitalOutputConfig; +import pibotlib.lib.addons.PwmConfig; +import pibotlib.lib.addons.RobotStateLight; import pibotlib.lib.addons.TimedRobotBase; +import pibotlib.lib.addons.sensors.UltraSonicSensor; +import pibotlib.lib.autonomous.TimedAutoBase; +import pibotlib.lib.autonomous.TimedCommand; +import pibotlib.lib.drives.DifferentialDrive; +import pibotlib.lib.gamecontrollers.LocalXboxController; +import pibotlib.lib.motorcontrollers.DualHBridgeController; public class Robot extends TimedRobotBase { Context context; + LocalXboxController controller; + DualHBridgeController leftController, rightController; + DifferentialDrive differentialDrive; + RobotStateLight stateLight; + UltraSonicSensor sonicSensor; + boolean controllerFound; - public Robot(){ + TimedAutoBase autoBase; + public Robot(){ + //constructor called once which can create its own controller + try { + controller = new LocalXboxController(); + controllerFound = true; + }catch (Exception e){ + System.out.println("No controller detected"); + controllerFound = false; + } } //called once per program init @Override public void robotInit() { + try { + context = Pi4J.newAutoContext();//always call first + + /* + * Declare motor controllers and their PWM and forward and back channels + */ + leftController = new DualHBridgeController(context, 14, 15, 23, 24); + rightController = new DualHBridgeController(context, 9, 25, 11, 8); + + leftController.configMotor1PWM(PwmConfig.buildPwmConfig(leftController.getContext(), 18, PwmType.HARDWARE)); + leftController.configMotor2PWM(PwmConfig.buildPwmConfig(leftController.getContext(), 12, PwmType.HARDWARE)); + rightController.configMotor1PWM(PwmConfig.buildPwmConfig(rightController.getContext(), 13, PwmType.HARDWARE)); + rightController.configMotor2PWM(PwmConfig.buildPwmConfig(rightController.getContext(), 19, PwmType.HARDWARE)); + + leftController.setMotor1DigitalForward(DigitalOutputConfig.buildDigitalOutputConfig(leftController.getContext(), 23, "pin23", "left motorL")); + leftController.setMotor1DigitalBackward(DigitalOutputConfig.buildDigitalOutputConfig(leftController.getContext(), 24, "pin24", "left motorL")); + leftController.setMotor2DigitalForward(DigitalOutputConfig.buildDigitalOutputConfig(leftController.getContext(), 25, "pin25", "left motorR")); + leftController.setMotor2DigitalBackward(DigitalOutputConfig.buildDigitalOutputConfig(leftController.getContext(), 8, "pin8", "left motorR")); + + rightController.setMotor1DigitalForward(DigitalOutputConfig.buildDigitalOutputConfig(rightController.getContext(), 17, "pin17", "left motorL")); + rightController.setMotor1DigitalBackward(DigitalOutputConfig.buildDigitalOutputConfig(rightController.getContext(), 27, "pin27", "left motorL")); + rightController.setMotor2DigitalForward(DigitalOutputConfig.buildDigitalOutputConfig(rightController.getContext(), 22, "pin22", "left motorR")); + rightController.setMotor2DigitalBackward(DigitalOutputConfig.buildDigitalOutputConfig(rightController.getContext(), 10, "pin10", "left motorR")); + + //If using a robot drive chassis you may create a differential drive object and pass your left and right controllers + differentialDrive = new DifferentialDrive(leftController, rightController); + //Add an RSL if you want to visually see if your robot is enabled/disabled + stateLight = new RobotStateLight(context, 16); + //Create an autobase object if you want to build an autonomous + autoBase = new TimedAutoBase(); + sonicSensor = new UltraSonicSensor(context,5,6,"Sonic Sensor"); + DriverStation.addSensor(sonicSensor); + + //activate the ultrasonic sensor + sonicSensor.runSensor(); + //make sure to add your autnomous commands in your constructor + autoBuild(); + + }catch (Exception e){ + System.out.println("Robot init fail, reboot raspberry pi and try again"); + e.printStackTrace(); + } + } + + private void autoBuild(){ + //add auto commands in the order you want them to execute + autoBase.addCommand(new TimedCommand(2000L,differentialDrive,0,80)); + autoBase.addCommand(new TimedCommand(2000L,differentialDrive,0,0)); } //called periodically at all times @Override public void robotPeriodic() { - + if (!controllerFound){ + try { + controller = new LocalXboxController(); + controllerFound = true; + }catch (Exception e){ + System.out.println("No controller found"); + e.printStackTrace(); + } + } } //called periodically during autonomous @Override public void autonomousPeriodic() { - + stateLight.blinkRSL(); + autoBase.runAuto(); } //called periodically during tele-op @Override public void teleopPeriodic() { - + differentialDrive.arcadeDrive(-controller.getLeftXAxis() * 100, controller.getRightYAxis() * 100); + stateLight.blinkRSL(); } - //called once when set to kill @Override public void robotShutDown() { - + stateLight.shutDown(); + differentialDrive.arcadeDrive(0, 0); + context.shutdown(); } } diff --git a/core/src/pibotlib/graphics/DriverStation.java b/core/src/pibotlib/graphics/DriverStation.java index 0201f5d..24ef81d 100644 --- a/core/src/pibotlib/graphics/DriverStation.java +++ b/core/src/pibotlib/graphics/DriverStation.java @@ -21,9 +21,10 @@ public class DriverStation implements Screen { TextureRegion img; DriverStationButton enableButton, disableButton , autoButton, teleopButton; com.badlogic.gdx.math.Rectangle mouseHitbox; - Font font, versionFont, interfaceFont; + Font font, versionFont, interfaceFont, displayableFont; LocalXboxController controller; static ArrayList sensors = new ArrayList<>(); + static ArrayList pidControllers = new ArrayList<>(); TimedRobotBase robot; boolean robotStarted; @@ -39,7 +40,13 @@ public void show() { font = new Font(100); versionFont = new Font(20); interfaceFont = new Font(35); - controller = new LocalXboxController(); + displayableFont = new Font(23); + + try { + controller = new LocalXboxController(); + }catch (Exception e){ + System.err.println("Xbox controller not found...");; + } } private void update(){ @@ -74,26 +81,35 @@ public void render(float delta) { font.draw(batch, DriverStationState.getRobotMode(),480,300,0,false); interfaceFont.draw(batch,"Controller: ",80,590,0,false); - interfaceFont.draw(batch,controller.getLeftXAxis() + " LX",80,520,0,false); - interfaceFont.draw(batch,controller.getLeftYAxis() + " LY",80,450,0,false); - interfaceFont.draw(batch,controller.getRightXAxis() + " RX",250,520,0,false); - interfaceFont.draw(batch,controller.getRightYAxis() + " RY",250,450,0,false); + + try { + interfaceFont.draw(batch, controller.getLeftXAxis() + " LX", 80, 520, 0, false); + interfaceFont.draw(batch, controller.getLeftYAxis() + " LY", 80, 450, 0, false); + interfaceFont.draw(batch, controller.getRightXAxis() + " RX", 250, 520, 0, false); + interfaceFont.draw(batch, controller.getRightYAxis() + " RY", 250, 450, 0, false); + }catch (Exception e){ + + } interfaceFont.draw(batch,"Sensors:",480,590,0,false);//sensors.get(0).getName() if ((sensors.size() >= 1)) { - interfaceFont.draw(batch, sensors.get(0).getName() + " :", 480, 520, 0, false); - interfaceFont.draw(batch, sensors.get(0).getValueToString(), 900, 520, 0, false); + displayableFont.draw(batch, sensors.get(0).getName() + " :", 420, 550, 0, false); + displayableFont.draw(batch, sensors.get(0).getValueToString(), 615, 550, 0, false); } if ((sensors.size() >= 2)) { - interfaceFont.draw(batch, sensors.get(1).getName() + " :", 480, 485, 0, false); - interfaceFont.draw(batch, sensors.get(1).getValueToString(), 900, 485, 0, false); + displayableFont.draw(batch, sensors.get(1).getName() + " :", 420, 485, 0, false); + displayableFont.draw(batch, sensors.get(1).getValueToString(), 615, 485, 0, false); } if ((sensors.size() == 3)) { - interfaceFont.draw(batch, sensors.get(2).getName() + " :", 480, 450, 0, false); - interfaceFont.draw(batch, sensors.get(2).getValueToString(), 900, 450, 0, false); + displayableFont.draw(batch, sensors.get(2).getName() + " :", 420, 450, 0, false); + displayableFont.draw(batch, sensors.get(2).getValueToString(), 615, 450, 0, false); } - + interfaceFont.draw(batch,"PID:",930,590,0,false); + if ((pidControllers.size() == 1)) { + displayableFont.draw(batch, pidControllers.get(0).getName() + " :", 850, 550, 0, false); + displayableFont.draw(batch, pidControllers.get(0).getValueToString(), 850, 520, 0, false); + } versionFont.draw(batch,"Version: " + Constants.LibConstants.LIB_VERSION ,920,20,0,false); batch.end(); } @@ -202,6 +218,14 @@ public static void addSensor(DisplayAble sensor){ } } + public static void addPidController(DisplayAble controller){ + if (pidControllers.size() == 1){ + System.out.println("Driver station sensor capacity full"); + }else { + pidControllers.add(controller); + } + } + public void setRobot(TimedRobotBase robot){ this.robot = robot; } diff --git a/core/src/pibotlib/lib/addons/TimedRobotBase.java b/core/src/pibotlib/lib/addons/TimedRobotBase.java index 18391d5..3e879ac 100644 --- a/core/src/pibotlib/lib/addons/TimedRobotBase.java +++ b/core/src/pibotlib/lib/addons/TimedRobotBase.java @@ -44,7 +44,7 @@ public void run() { System.exit(0); } } - }, 0, 10); + }, 0, 35); } @Override diff --git a/core/src/pibotlib/lib/addons/sensors/UltraSonicSensor.java b/core/src/pibotlib/lib/addons/sensors/UltraSonicSensor.java index cfed5a5..7817ff6 100644 --- a/core/src/pibotlib/lib/addons/sensors/UltraSonicSensor.java +++ b/core/src/pibotlib/lib/addons/sensors/UltraSonicSensor.java @@ -70,7 +70,7 @@ public void run() { e.printStackTrace(); } } - }, 0, 10); + }, 0, 35); } } diff --git a/core/src/pibotlib/lib/autonomous/TimedCommand.java b/core/src/pibotlib/lib/autonomous/TimedCommand.java index bfbdfb6..3af7b50 100644 --- a/core/src/pibotlib/lib/autonomous/TimedCommand.java +++ b/core/src/pibotlib/lib/autonomous/TimedCommand.java @@ -48,7 +48,7 @@ public void run() { taskTimer.cancel(); } } - }, 0, 10); + }, 0, 35); } diff --git a/core/src/pibotlib/lib/constants/Constants.java b/core/src/pibotlib/lib/constants/Constants.java index ff54fd9..85a3600 100644 --- a/core/src/pibotlib/lib/constants/Constants.java +++ b/core/src/pibotlib/lib/constants/Constants.java @@ -6,7 +6,7 @@ public class Constants { public static class LibConstants { - public final static String LIB_VERSION = "1.1.0"; + public final static String LIB_VERSION = "1.1.2"; } public static class Graphical { diff --git a/core/src/pibotlib/lib/pid/PIDController.java b/core/src/pibotlib/lib/pid/PIDController.java index 6539c7c..79814f7 100644 --- a/core/src/pibotlib/lib/pid/PIDController.java +++ b/core/src/pibotlib/lib/pid/PIDController.java @@ -21,7 +21,6 @@ public class PIDController implements DisplayAble { volatile double errorSum = 0; volatile double errorRate = 0; volatile double lastError = 0; - volatile double lastTimeStamp = 0; volatile double currentTime = 0; volatile double sensorValue; @@ -60,9 +59,17 @@ public PIDController(double kP, double kI, double kD, double iLimit, double maxT timer = new ElapseTimer(); } + boolean timerStarted = false; + public double getOutput(double sensorValue){ - timer.startTimer(); - currentTime = timer.getElapsedMilliseconds() - lastTimeStamp; + + this.sensorValue = sensorValue; + + if (!timerStarted){ + timer.startTimer(); + timerStarted = true; + } + currentTime = timer.getElapsedMilliseconds(); if(sensorValue > target){ error = target - sensorValue; greaterThanMax = true; @@ -80,7 +87,6 @@ public double getOutput(double sensorValue){ } errorRate = (error - lastError)/currentTime; - lastTimeStamp = timer.getElapsedMilliseconds(); lastError = error; isOnTarget = maxTarget >= sensorValue && minTarget <= sensorValue; @@ -91,9 +97,12 @@ public double getOutput(double sensorValue){ errorRate = 0; timer.reset(); timer.stopTimer(); + timerStarted = false; return 0; } - return (kP * error) + (kI * errorSum) + (kD * errorRate);//only PI for now + + output = (kP * error) + (kI * errorSum) + (kD * errorRate); + return output;//only PI for now } public void pause(){ @@ -102,6 +111,7 @@ public void pause(){ errorRate = 0; timer.reset(); timer.stopTimer(); + timerStarted = false; } public void setTarget(double minTolerance, double maxTolerance, double target){ @@ -124,7 +134,6 @@ public void setTarget(double minTolerance, double maxTolerance, double target){ public synchronized double getError(){return error;} public synchronized double getErrorSum(){return errorSum;} - public synchronized double getLastTimeStamp(){return lastTimeStamp;} public synchronized double getCurrentTime(){return currentTime;} public synchronized double getSensorValue(){return sensorValue;} @@ -135,7 +144,7 @@ public void setTarget(double minTolerance, double maxTolerance, double target){ @Override public String getValueToString() { - return null; + return "error: " + getError() + "\n" + "sensor: " + getSensorValue() + "\n" + "output: " + output + "\n" + "is on target: " + isOnTarget() + "\n" + " Elapsed: " + getCurrentTime(); } @Override diff --git a/core/src/pibotlib/lib/time/ElapseTimer.java b/core/src/pibotlib/lib/time/ElapseTimer.java index 6aaa3c6..c54df08 100644 --- a/core/src/pibotlib/lib/time/ElapseTimer.java +++ b/core/src/pibotlib/lib/time/ElapseTimer.java @@ -53,15 +53,15 @@ public synchronized double getElapsedSeconds(){ } @Override - public void run() {//TODO check this works + public void run() { ScheduledExecutorService executor = Executors.newScheduledThreadPool(1); Runnable helloRunnable = () -> { - elapsedMilliseconds += 1; + elapsedMilliseconds += 100; elapsedSeconds = elapsedMilliseconds/1000d; if (stopTimer){ executor.shutdown(); } }; - executor.scheduleAtFixedRate(helloRunnable, 0, 1, TimeUnit.MILLISECONDS); + executor.scheduleAtFixedRate(helloRunnable, 0, 100, TimeUnit.MILLISECONDS); } }