<commit_before>
package ch.unizh.ini.tobi.rccar;

import ch.unizh.ini.caviar.chip.*;
import ch.unizh.ini.caviar.event.*;
import ch.unizh.ini.caviar.event.EventPacket;
import ch.unizh.ini.caviar.eventio.*;
import ch.unizh.ini.caviar.hardwareinterface.*;
import ch.unizh.ini.caviar.hardwareinterface.usb.toradex.ToradexOakG3AxisAccelerationSensor;
import ch.unizh.ini.caviar.hardwareinterface.usb.toradex.ToradexOakG3AxisAccelerationSensor.Acceleration;
import ch.unizh.ini.caviar.hardwareinterface.usb.toradex.ToradexOakG3AxisAccelerationSensorGUI;
//import ch.unizh.ini.caviar.eventio.AEServerSocket;
import ch.unizh.ini.caviar.eventprocessing.*;
import ch.unizh.ini.caviar.eventprocessing.filter.*;
import ch.unizh.ini.caviar.eventprocessing.label.*;
//import ch.unizh.ini.caviar.eventprocessing.label.SimpleOrientationFilter;
import ch.unizh.ini.caviar.eventprocessing.tracking.*;
import ch.unizh.ini.caviar.eventprocessing.tracking.HoughLineTracker;
import ch.unizh.ini.caviar.eventprocessing.tracking.MultiLineClusterTracker;
import ch.unizh.ini.caviar.graphics.FrameAnnotater;
import ch.unizh.ini.caviar.hardwareinterface.*;
//import ch.unizh.ini.caviar.util.filter.LowpassFilter;
import java.awt.Graphics2D;
import java.beans.*;
import java.io.*;
import java.io.BufferedOutputStream;
import java.io.DataOutputStream;
import java.net.Socket;
import java.util.logging.*;
import java.util.prefs.*;
import javax.media.opengl.*;
import javax.media.opengl.GLAutoDrawable;
import javax.media.opengl.glu.*;

/**
 * The FancyDriver controls the RcCar 

 
 * @author <NAME>
 */
public class FancyDriver extends EventFilter2D implements FrameAnnotater{
        
    private boolean flipSteering=getPrefs().getBoolean("Driver.flipSteering",true);
    {setPropertyTooltip("flipSteering","flips the steering command for use with mirrored scene");}
    
    private boolean useMultiLineTracker=getPrefs().getBoolean("Driver.useMultiLineTracker",true);
    {setPropertyTooltip("useMultiLineTracker","enable to use MultiLineClusterTracker, disable to use HoughLineTracker");}
    
    private float steerDecay=getPrefs().getFloat("Driver.steerDecay",0.4f);
    {setPropertyTooltip("steerDecay","time constant in ms for driving to far-away line");}
    
    private float defaultSpeed=getPrefs().getFloat("Driver.defaultSpeed",0f); // speed of car when filter is turned on
    {setPropertyTooltip("defaultSpeed","Car will drive with this fwd speed when filter is enabled");}
    
    private boolean sendControlToBlenderEnabled=true;
    {setPropertyTooltip("sendControlToBlenderEnabled","sends steering (controlled) and speed (from radio) to albert's blender client");}
    
    private float lateralGain=getPrefs().getFloat("Driver.lateralGain",0.005f);
    {setPropertyTooltip("lateralGain","gain for moving back to the line");}
    
    private float angleGain=getPrefs().getFloat("Driver.angleGain",0.5f);
    {setPropertyTooltip("angleGain","gain for aligning with the line");}
    
    private boolean showAccelerometerGUI = false;
    {setPropertyTooltip("showAccelerometerGUI", "shows the GUI output for the accelerometer");}
    
    // Variables for the fancy pid controller
    private float Kp=1f; // proportinal gain of the pid controller
    private float Ki=1f; // integral gain of the pid controller
    private float Kd=1f; // derivative gain of the pid controller
    private float IntError=0f; // integral of the weighted error
    private float LastWeightedError=0f; // weighted error of last call
    private int LastTimestamp=0; // timestamp of last package
    private float SteeringRange=(float)Math.PI/2; // range of the servo steering
    
    DrivingController controller;
    //static Logger log=Logger.getLogger("FancyDriver");
    private SiLabsC8051F320_USBIO_CarServoController servo;
    private ToradexOakG3AxisAccelerationSensor accelerometer;
    private ToradexOakG3AxisAccelerationSensorGUI acceleromterGUI=null;
    private HingeLineTracker lineTracker;
    private float radioSteer=0.5f;
    private float radioSpeed=0.5f;
    private float steerCommand = 0.5f;
    private float leftPhi = 0;
    private float rightPhi = 0;
    private float leftX = -2;
    private float rightX = 2;
    private float lateralCorrection = 0;
    private float angularCorrection = 0;


    /** Creates a new instance of FancyDriver */
    public FancyDriver(AEChip chip) {
        super(chip);
        chip.getCanvas().addAnnotator(this);
        initFilter();
        controller=new DrivingController();
        accelerometer = new ToradexOakG3AxisAccelerationSensor();
        try {
            accelerometer.open();
        } catch (HardwareInterfaceException ex) {
            log.warning(ex.toString());
        }
        
        
    }
    
    private class DrivingController{
        
        void control(EventPacket in){
            // get values from radio receiver (user sets speed or steers)
            if(servo!=null){
                radioSteer=servo.getRadioSteer();
                radioSpeed=servo.getRadioSpeed();
            }
//            if (accelerometer != null) {
//                //accel is a acceleration vector with the float components accel.x, accel.y, accel.z and accel.t
//                Acceleration accel = accelerometer.getAcceleration();
//            }
//            if(lineTracker != null){
//                leftPhi = lineTracker.getLeftPhi();
//                rightPhi = lineTracker.getRightPhi();
//                leftX = lineTracker.getLeftX();
//                rightX = lineTracker.getRightX();
//                
//                if(leftX > 0)
//                lateralCorrection = lateralCorrection+leftX;
//                if(rightX < 0)
//                lateralCorrection = lateralCorrection+rightX;
//                
//                angularCorrection = angularCorrection+rightPhi+leftPhi;
//            }
//            
//            steerCommand = 0.5f + (steerCommand-0.5f)*steerDecay;
//            
//            steerCommand = steerCommand+lateralCorrection*lateralGain+angularCorrection*angleGain;
//            
//            if (steerCommand < 0) {
//                steerCommand = 0;
//            }
//            if (steerCommand > 1) {
//                steerCommand = 1;
//            }
//
//            if(servo!=null && servo.isOpen()){
//                servo.setSteering(0.5f); 
//                servo.setSpeed(getDefaultSpeed()+0.5f); // set fwd speed
//            }
   
            // Get weighted error
            float WeightedError=getWeightedError();
            
            // Calculate time since last packet in seconds
            float DeltaT = (in.getLastTimestamp() - LastTimestamp)/1000000f; // time in seconds since last packet
            LastTimestamp = in.getLastTimestamp();
            
            // Calculate proportional component
            float ComponentKp = -Kp*WeightedError;
            
            // Calculate integral component
            IntError = IntError+DeltaT*(LastWeightedError+WeightedError)/2;
            float ComponentKi = -Ki*IntError;
            
            // Calculate derivative component
            float Derivative = (WeightedError-LastWeightedError)/DeltaT;
            float ComponentKd = -Kd*Derivative;
            
            // Calculate u
            float u = ComponentKp+ComponentKi+ComponentKd;
            
            // Normalize u
            u = u/SteeringRange;
            
            // Check for limits
            if(u < -1){
                u = -1f;
            }
            if(u > 1){
                u = 1f;
            }
            
            // Send steering command
            steerCommand = u;
            steerCommand = 0.2f;
            if(servo!=null && servo.isOpen()){
                servo.setSteering(getSteerCommand()); // -1 steer right, 1 steer left<commit_msg>New version of the Fancy Driver<commit_after>
package ch.unizh.ini.tobi.rccar;

import ch.unizh.ini.caviar.chip.*;
import ch.unizh.ini.caviar.event.*;
import ch.unizh.ini.caviar.event.EventPacket;
import ch.unizh.ini.caviar.eventio.*;
import ch.unizh.ini.caviar.hardwareinterface.*;
import ch.unizh.ini.caviar.hardwareinterface.usb.toradex.ToradexOakG3AxisAccelerationSensor;
import ch.unizh.ini.caviar.hardwareinterface.usb.toradex.ToradexOakG3AxisAccelerationSensor.Acceleration;
import ch.unizh.ini.caviar.hardwareinterface.usb.toradex.ToradexOakG3AxisAccelerationSensorGUI;
//import ch.unizh.ini.caviar.eventio.AEServerSocket;
import ch.unizh.ini.caviar.eventprocessing.*;
import ch.unizh.ini.caviar.eventprocessing.filter.*;
import ch.unizh.ini.caviar.eventprocessing.label.*;
//import ch.unizh.ini.caviar.eventprocessing.label.SimpleOrientationFilter;
import ch.unizh.ini.caviar.eventprocessing.tracking.*;
import ch.unizh.ini.caviar.eventprocessing.tracking.HoughLineTracker;
import ch.unizh.ini.caviar.eventprocessing.tracking.MultiLineClusterTracker;
import ch.unizh.ini.caviar.graphics.FrameAnnotater;
import ch.unizh.ini.caviar.hardwareinterface.*;
//import ch.unizh.ini.caviar.util.filter.LowpassFilter;
import java.awt.Graphics2D;
import java.beans.*;
import java.io.*;
import java.io.BufferedOutputStream;
import java.io.DataOutputStream;
import java.net.Socket;
import java.util.logging.*;
import java.util.prefs.*;
import javax.media.opengl.*;
import javax.media.opengl.GLAutoDrawable;
import javax.media.opengl.glu.*;

/**
 * The FancyDriver controls the RcCar 

 
 * @author <NAME>
 */
public class FancyDriver extends EventFilter2D implements FrameAnnotater{
        
    private boolean flipSteering=getPrefs().getBoolean("Driver.flipSteering",true);
    {setPropertyTooltip("flipSteering","flips the steering command for use with mirrored scene");}
    
    private boolean useMultiLineTracker=getPrefs().getBoolean("Driver.useMultiLineTracker",true);
    {setPropertyTooltip("useMultiLineTracker","enable to use MultiLineClusterTracker, disable to use HoughLineTracker");}
    
    private float steerDecay=getPrefs().getFloat("Driver.steerDecay",0.4f);
    {setPropertyTooltip("steerDecay","time constant in ms for driving to far-away line");}
    
    private float defaultSpeed=getPrefs().getFloat("Driver.defaultSpeed",0f); // speed of car when filter is turned on
    {setPropertyTooltip("defaultSpeed","Car will drive with this fwd speed when filter is enabled");}
    
    private boolean sendControlToBlenderEnabled=true;
    {setPropertyTooltip("sendControlToBlenderEnabled","sends steering (controlled) and speed (from radio) to albert's blender client");}
    
    private float lateralGain=getPrefs().getFloat("Driver.lateralGain",0.005f);
    {setPropertyTooltip("lateralGain","gain for moving back to the line");}
    
    private float angleGain=getPrefs().getFloat("Driver.angleGain",0.5f);
    {setPropertyTooltip("angleGain","gain for aligning with the line");}
    
    private boolean showAccelerometerGUI = false;
    {setPropertyTooltip("showAccelerometerGUI", "shows the GUI output for the accelerometer");}
    
    // Variables for the fancy pid controller
    private float Kp=1f; // proportinal gain of the pid controller
    private float Ki=1f; // integral gain of the pid controller
    private float Kd=1f; // derivative gain of the pid controller
    private float IntError=0f; // integral of the weighted error
    private float LastWeightedError=0f; // weighted error of last call
    private int LastTimestamp=0; // timestamp of last package
    private float SteeringRange=(float)Math.PI/2; // range of the servo steering
    
    DrivingController controller;
    //static Logger log=Logger.getLogger("FancyDriver");
    private SiLabsC8051F320_USBIO_CarServoController servo;
    private ToradexOakG3AxisAccelerationSensor accelerometer;
    private ToradexOakG3AxisAccelerationSensorGUI acceleromterGUI=null;
    private HingeLineTracker lineTracker;
    private float radioSteer=0.5f;
    private float radioSpeed=0.5f;
    private float steerCommand = 0.5f;
    private float leftPhi = 0;
    private float rightPhi = 0;
    private float leftX = -2;
    private float rightX = 2;
    private float lateralCorrection = 0;
    private float angularCorrection = 0;


    /** Creates a new instance of FancyDriver */
    public FancyDriver(AEChip chip) {
        super(chip);
        chip.getCanvas().addAnnotator(this);
        initFilter();
        controller=new DrivingController();
        accelerometer = new ToradexOakG3AxisAccelerationSensor();
        try {
            accelerometer.open();
        } catch (HardwareInterfaceException ex) {
            log.warning(ex.toString());
        }
        
        
    }
    
    private class DrivingController{
        
        void control(EventPacket in){
            // get values from radio receiver (user sets speed or steers)
            if(servo!=null){
                radioSteer=servo.getRadioSteer();
                radioSpeed=servo.getRadioSpeed();
            }
//            if (accelerometer != null) {
//                //accel is a acceleration vector with the float components accel.x, accel.y, accel.z and accel.t
//                Acceleration accel = accelerometer.getAcceleration();
//            }
//            if(lineTracker != null){
//                leftPhi = lineTracker.getLeftPhi();
//                rightPhi = lineTracker.getRightPhi();
//                leftX = lineTracker.getLeftX();
//                rightX = lineTracker.getRightX();
//                
//                if(leftX > 0)
//                lateralCorrection = lateralCorrection+leftX;
//                if(rightX < 0)
//                lateralCorrection = lateralCorrection+rightX;
//                
//                angularCorrection = angularCorrection+rightPhi+leftPhi;
//            }
//            
//            steerCommand = 0.5f + (steerCommand-0.5f)*steerDecay;
//            
//            steerCommand = steerCommand+lateralCorrection*lateralGain+angularCorrection*angleGain;
//            
//            if (steerCommand < 0) {
//                steerCommand = 0;
//            }
//            if (steerCommand > 1) {
//                steerCommand = 1;
//            }
//
//            if(servo!=null && servo.isOpen()){
//                servo.setSteering(0.5f); 
//                servo.setSpeed(getDefaultSpeed()+0.5f); // set fwd speed
//            }
   
            // Get weighted error
            float WeightedError=getWeightedError();
            
            // Calculate time since last packet in seconds
            float DeltaT = (in.getLastTimestamp() - LastTimestamp)/1000000f; // time in seconds since last packet
            LastTimestamp = in.getLastTimestamp();
            
            // Calculate proportional component
            float ComponentKp = -Kp*WeightedError;
            
            // Calculate integral component
            IntError = IntError+DeltaT*(LastWeightedError+WeightedError)/2;
            float ComponentKi = -Ki*IntError;
            
            // Calculate derivative component
            float Derivative = (WeightedError-LastWeightedError)/DeltaT;
            float ComponentKd = -Kd*Derivative;
            
            // Calculate u
            float u = ComponentKp+ComponentKi+ComponentKd;
            
            // Normalize u
            u = u/SteeringRange;
            
            // Check for limits
            if(u < -1){
                u = -1f;
            }
            if(u > 1){
                u = 1f;
            }
            
            // Send steering command
            steerCommand = u;
            float servoSteerCommand = - (u/2) + 0.5f; 
            if(servo!=null && servo.isOpen()){
                servo.setSteering(servoSteerCommand); // 1 steer right, 0 steer left
{'fraction_non_alphanumeric': 0.10235570512413486, 'fraction_numerical': 0.009333925963553242, 'mean_word_length': 2.4752868490732567, 'pattern_counts': {'":': 0, '<': 11, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '4895184', 'n_tokens_mistral': 4683, 'n_tokens_neox': 4455, 'n_words': 968}