Skip to content

Implemented direct joystick logs #22

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
alexanderlindquis wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Implemented direct joystick logs #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 15 additions & 0 deletions MicrocontrollerCode/include/JoystickFunctions.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,13 +15,28 @@ struct RefSpeed {
int8_t rightSpeed; ///< Speed of the right wheel.
};

/**
* Struct representing the reference displace
*/
struct RefDisplacement {
int8_t longDisp; ///< Forward/Backward displacement with + indicating forward
int8_t latDisp; ///< Side to side displacement with + indicating right
};

/**
* Reads a value from the joystick connected to the ADC and returns the reference speeds
* @param adc An instance of the adc the joystick is connected to
* @return A RefSpeed for the wheelchair containing the wheel speeds and the direction
*/
RefSpeed joystickToSpeed(Adafruit_ADS1115 &adc);

/**
* Reads a value from the joystick connected to the ADC and returns the reference displacement
* @param adc An instance of the adc the joystick is connected to
* @return A RefDisplacement for the joystick
*/
RefDisplacement joystickToDisplacement(Adafruit_ADS1115 &adc);

template <typename T>
/**
* Custom clamp function to keep a value between to values
Expand Down
22 changes: 22 additions & 0 deletions MicrocontrollerCode/src/JoystickFunctions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,28 @@
int diffParam = 30;
int deadzoneParam = 30;

RefDisplacement joystickToDisplacement(Adafruit_ADS1115 &adc){
int forwardJoystick = adc.readADC_SingleEnded(0); //a0 is forward/backward
int sidewaysJoystick = adc.readADC_SingleEnded(1); //a1 is left/right

RefDisplacement displacements;
/*
* Joystick middle values: ~8500
* a0 middle value: ~8500
* a1 middle value: ~8300
* a0 deadzone 10000 - 6500
* a1 deadzone 11000 - 6000
* Joystick Min: 0
* Joystick Max: 17390
* Output is a value -100 to 100 for the speed of the motor
*/

//Converting the speeds so they start around 0 and then go positive and negative
displacements.longDisp = forwardJoystick - (8500+4400); //The second value is used to zero it out when the ADC gain is set to 0 instead of the default (2/3)
displacements.latDisp = sidewaysJoystick - (8400+4400);
return displacements;
}

RefSpeed joystickToSpeed(Adafruit_ADS1115 &adc){
int forwardJoystick = adc.readADC_SingleEnded(0); //a0 is forward/backward
int sidewaysJoystick = adc.readADC_SingleEnded(1); //a1 is left/right
Expand Down
8 changes: 6 additions & 2 deletions MicrocontrollerCode/src/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,10 +104,14 @@ void loop() {
//TODO might want to figure out how to put these on core1 so that they can run in parallel
//uint32_t start = millis();
RefSpeed omegaRef{};
RefDisplacement thetaRef{};
if (!joystick_adc_error) {
omegaRef = joystickToSpeed(joystickAdc);
thetaRef = joystickToDisplacement(joystickAdc);
}




//uint32_t joystickTime = millis() - start;
USData usDistances{};
Expand Down Expand Up @@ -147,7 +151,7 @@ void loop() {

microRosTick();

transmitMsg(omegaRef, usDistances, pirSensors, fanSpeeds, imuData);
transmitMsg(thetaRef,omegaRef, usDistances, pirSensors, fanSpeeds, imuData);

if (currentMillis - lastErrorTime >= error_timer) {
lastErrorTime = currentMillis;
Expand All @@ -160,7 +164,7 @@ void loop() {
#elif ROS_DEBUG


transmitMsg(omegaRef);
transmitMsg(thetaRef,omegaRef);


#elif DEBUG
Expand Down
8 changes: 6 additions & 2 deletions MicrocontrollerCode/src/microRosFunctions.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -373,7 +373,9 @@ static void lidar_subscription_callback(const void *msgin){
}

#ifdef ROS
void transmitMsg(RefSpeed omegaRef, USData ultrasonicData, PIRSensors pirSensors, FanSpeeds fanSpeeds, IMUData imuData) {
void transmitMsg(RefDisplacement thetaRef, RefSpeed omegaRef, USData ultrasonicData, PIRSensors pirSensors, FanSpeeds fanSpeeds, IMUData imuData) {
sensorMsg.long_disp = thetaRef.longDisp;
sensorMsg.lat_disp = thetaRef.latDisp;
sensorMsg.left_speed = omegaRef.leftSpeed;
sensorMsg.right_speed = omegaRef.rightSpeed;
sensorMsg.ultrasonic_front_0 = ultrasonicData.us_front_0;
Expand Down Expand Up @@ -406,9 +408,11 @@ void transmitMsg(RefSpeed omegaRef, USData ultrasonicData, PIRSensors pirSensors

#elif ROS_DEBUG

void transmitMsg(RefSpeed omegaRef){
void transmitMsg(RefDisplacement thetaRef, RefSpeed omegaRef){
msg.left_speed = omegaRef.leftSpeed;
msg.right_speed = omegaRef.rightSpeed;
msg.long_disp = thetaRef.longDisp;
msg.lat_disp = thetaRef.latDisp;

RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(10)));
}
Expand Down