From 5a938c7720436670063878ef0cfed07e765fb195 Mon Sep 17 00:00:00 2001 From: Giovanni Bruno Date: Wed, 9 Oct 2024 15:49:16 +0200 Subject: [PATCH] battery alert --- examples/firmware/firmware.ino | 207 ++++++++++++++-------------- src/Arduino_AlvikCarrier.cpp | 19 ++- src/Arduino_AlvikCarrier.h | 3 +- src/definitions/robot_definitions.h | 3 +- 4 files changed, 125 insertions(+), 107 deletions(-) diff --git a/examples/firmware/firmware.ino b/examples/firmware/firmware.ino index 66843a8..cb998a1 100644 --- a/examples/firmware/firmware.ino +++ b/examples/firmware/firmware.ino @@ -96,110 +96,113 @@ void loop(){ } if (packeter.checkPayload()) { code = packeter.payloadTop(); - switch (code){ - case 'J': - packeter.unpacketC2F(code,left,right); - alvik.disableKinematicsMovement(); - alvik.disablePositionControl(); - alvik.setRpm(left, right); - break; - - case 'V': - packeter.unpacketC2F(code,linear,angular); - alvik.disableKinematicsMovement(); - alvik.disablePositionControl(); - alvik.drive(linear,angular); - break; - - case 'W': - packeter.unpacketC2B1F(code,label,control_type,value); - alvik.disableKinematicsMovement(); - if (label=='L'){ - switch (control_type){ - case 'V': - alvik.disablePositionControlLeft(); - alvik.setRpmLeft(value); - break; - case 'P': - alvik.setPositionLeft(value); - ack_required=MOVEMENT_LEFT; - ack_check=true; - break; - case 'Z': - alvik.resetPositionLeft(value); - break; + if (!alvik.isBatteryAlert()){ + switch (code){ + case 'J': + packeter.unpacketC2F(code,left,right); + alvik.disableKinematicsMovement(); + alvik.disablePositionControl(); + alvik.setRpm(left, right); + break; + + case 'V': + packeter.unpacketC2F(code,linear,angular); + alvik.disableKinematicsMovement(); + alvik.disablePositionControl(); + alvik.drive(linear,angular); + break; + + case 'W': + packeter.unpacketC2B1F(code,label,control_type,value); + alvik.disableKinematicsMovement(); + if (label=='L'){ + switch (control_type){ + case 'V': + alvik.disablePositionControlLeft(); + alvik.setRpmLeft(value); + break; + case 'P': + alvik.setPositionLeft(value); + ack_required=MOVEMENT_LEFT; + ack_check=true; + break; + case 'Z': + alvik.resetPositionLeft(value); + break; + } } - } - if (label=='R'){ - switch (control_type){ - case 'V': - alvik.disablePositionControlRight(); - alvik.setRpmRight(value); - break; - case 'P': - alvik.setPositionRight(value); - ack_required=MOVEMENT_RIGHT; - ack_check=true; - break; - case 'Z': - alvik.resetPositionRight(value); - break; + if (label=='R'){ + switch (control_type){ + case 'V': + alvik.disablePositionControlRight(); + alvik.setRpmRight(value); + break; + case 'P': + alvik.setPositionRight(value); + ack_required=MOVEMENT_RIGHT; + ack_check=true; + break; + case 'Z': + alvik.resetPositionRight(value); + break; + } } - } - break; - - - case 'A': - packeter.unpacketC2F(code,position_left, position_right); - alvik.disableKinematicsMovement(); - alvik.setPosition(position_left, position_right); - ack_required=MOVEMENT_POSITION; - ack_check=true; - break; - - - case 'S': - packeter.unpacketC2B(code,servo_A,servo_B); - alvik.setServoA(servo_A); - alvik.setServoB(servo_B); - break; - - case 'L': - packeter.unpacketC1B(code,leds); - alvik.setAllLeds(leds); - break; - - case 'P': - packeter.unpacketC1B3F(code,pid,kp,ki,kd); - if (pid=='L'){ - alvik.setKPidLeft(kp,ki,kd); - } - if (pid=='R'){ - alvik.setKPidRight(kp,ki,kd); - } - break; - - case 'R': - packeter.unpacketC1F(code, value); - alvik.disablePositionControl(); - alvik.rotate(value); - ack_required=MOVEMENT_ROTATE; - ack_check=true; - break; - - case 'G': - packeter.unpacketC1F(code, value); - alvik.disablePositionControl(); - alvik.move(value); - ack_required=MOVEMENT_MOVE; - ack_check=true; - break; - - case 'Z': - packeter.unpacketC3F(code, x, y, theta); - alvik.resetPose(x, y, theta); - break; - + break; + + + case 'A': + packeter.unpacketC2F(code,position_left, position_right); + alvik.disableKinematicsMovement(); + alvik.setPosition(position_left, position_right); + ack_required=MOVEMENT_POSITION; + ack_check=true; + break; + + + case 'S': + packeter.unpacketC2B(code,servo_A,servo_B); + alvik.setServoA(servo_A); + alvik.setServoB(servo_B); + break; + + case 'L': + packeter.unpacketC1B(code,leds); + alvik.setAllLeds(leds); + break; + + case 'P': + packeter.unpacketC1B3F(code,pid,kp,ki,kd); + if (pid=='L'){ + alvik.setKPidLeft(kp,ki,kd); + } + if (pid=='R'){ + alvik.setKPidRight(kp,ki,kd); + } + break; + + case 'R': + packeter.unpacketC1F(code, value); + alvik.disablePositionControl(); + alvik.rotate(value); + ack_required=MOVEMENT_ROTATE; + ack_check=true; + break; + + case 'G': + packeter.unpacketC1F(code, value); + alvik.disablePositionControl(); + alvik.move(value); + ack_required=MOVEMENT_MOVE; + ack_check=true; + break; + + case 'Z': + packeter.unpacketC3F(code, x, y, theta); + alvik.resetPose(x, y, theta); + break; + } + } + switch (code){ case 'X': packeter.unpacketC1B(code, ack_code); if (ack_code == 'K') { diff --git a/src/Arduino_AlvikCarrier.cpp b/src/Arduino_AlvikCarrier.cpp index 71d5e13..f72b692 100644 --- a/src/Arduino_AlvikCarrier.cpp +++ b/src/Arduino_AlvikCarrier.cpp @@ -298,6 +298,10 @@ float Arduino_AlvikCarrier::isBatteryCharging(){ return charging; } +bool Arduino_AlvikCarrier::isBatteryAlert(){ + return battery_alert; +} + /******************************************************************************************************/ @@ -982,6 +986,7 @@ void Arduino_AlvikCarrier::beginBehaviours(){ first_lift = true; battery_alert_time = millis(); battery_alert_wave = 100; + battery_alert = false; } @@ -1007,7 +1012,7 @@ void Arduino_AlvikCarrier::updateBehaviours(){ // battery alert if ((1<<(BATTERY_ALERT-1)) & behaviours){ - if (getBatteryVoltage()battery_alert_wave){ battery_alert_time = millis(); if (battery_alert_wave==400){ @@ -1018,11 +1023,19 @@ void Arduino_AlvikCarrier::updateBehaviours(){ setLeds(COLOR_BLACK); battery_alert_wave=400; } + } + if (getBatteryChargePercentage()stop(); - motor_right->stop(); + setIlluminator(false); } } + else{ + battery_alert = false; + } + } + else{ + battery_alert = false; } } diff --git a/src/Arduino_AlvikCarrier.h b/src/Arduino_AlvikCarrier.h index a1d50c9..17c0165 100644 --- a/src/Arduino_AlvikCarrier.h +++ b/src/Arduino_AlvikCarrier.h @@ -96,6 +96,7 @@ class Arduino_AlvikCarrier{ bool first_lift; unsigned long battery_alert_time; unsigned long battery_alert_wave; + bool battery_alert; @@ -159,6 +160,7 @@ class Arduino_AlvikCarrier{ float getBatteryVoltage(); // get Voltage float getBatteryChargePercentage(); // get battery percentage float isBatteryCharging(); // return -1 if battery discharge or 1 if charging + bool isBatteryAlert(); // return true if battery is LOW and alert behaviour is setted, otherwise false @@ -190,7 +192,6 @@ class Arduino_AlvikCarrier{ - // Touch int beginTouch(); // initialize touch void updateTouch(); // update touch status diff --git a/src/definitions/robot_definitions.h b/src/definitions/robot_definitions.h index a3b8c6d..ed52ae2 100644 --- a/src/definitions/robot_definitions.h +++ b/src/definitions/robot_definitions.h @@ -96,7 +96,8 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ); #define VERSION_BYTE_LOW 3 // Battery stats -#define BATTERY_ALERT_MINIMUM_VOLTAGE 4.0 +#define BATTERY_ALERT_MINIMUM_CHARGE 96.0 +#define BATTERY_ALERT_STOP_CHARGE 94.0