Skip to content

Commit 5a938c7

Browse files
gbr1gbr1
committed
battery alert
1 parent bfb9cf7 commit 5a938c7

File tree

4 files changed

+125
-107
lines changed

4 files changed

+125
-107
lines changed

‎examples/firmware/firmware.ino

Lines changed: 105 additions & 102 deletions
Original file line numberDiff line numberDiff line change
@@ -96,110 +96,113 @@ void loop(){
9696
}
9797
if (packeter.checkPayload()) {
9898
code = packeter.payloadTop();
99-
switch (code){
100-
case 'J':
101-
packeter.unpacketC2F(code,left,right);
102-
alvik.disableKinematicsMovement();
103-
alvik.disablePositionControl();
104-
alvik.setRpm(left, right);
105-
break;
106-
107-
case 'V':
108-
packeter.unpacketC2F(code,linear,angular);
109-
alvik.disableKinematicsMovement();
110-
alvik.disablePositionControl();
111-
alvik.drive(linear,angular);
112-
break;
113-
114-
case 'W':
115-
packeter.unpacketC2B1F(code,label,control_type,value);
116-
alvik.disableKinematicsMovement();
117-
if (label=='L'){
118-
switch (control_type){
119-
case 'V':
120-
alvik.disablePositionControlLeft();
121-
alvik.setRpmLeft(value);
122-
break;
123-
case 'P':
124-
alvik.setPositionLeft(value);
125-
ack_required=MOVEMENT_LEFT;
126-
ack_check=true;
127-
break;
128-
case 'Z':
129-
alvik.resetPositionLeft(value);
130-
break;
99+
if (!alvik.isBatteryAlert()){
100+
switch (code){
101+
case 'J':
102+
packeter.unpacketC2F(code,left,right);
103+
alvik.disableKinematicsMovement();
104+
alvik.disablePositionControl();
105+
alvik.setRpm(left, right);
106+
break;
107+
108+
case 'V':
109+
packeter.unpacketC2F(code,linear,angular);
110+
alvik.disableKinematicsMovement();
111+
alvik.disablePositionControl();
112+
alvik.drive(linear,angular);
113+
break;
114+
115+
case 'W':
116+
packeter.unpacketC2B1F(code,label,control_type,value);
117+
alvik.disableKinematicsMovement();
118+
if (label=='L'){
119+
switch (control_type){
120+
case 'V':
121+
alvik.disablePositionControlLeft();
122+
alvik.setRpmLeft(value);
123+
break;
124+
case 'P':
125+
alvik.setPositionLeft(value);
126+
ack_required=MOVEMENT_LEFT;
127+
ack_check=true;
128+
break;
129+
case 'Z':
130+
alvik.resetPositionLeft(value);
131+
break;
132+
}
131133
}
132-
}
133-
if (label=='R'){
134-
switch (control_type){
135-
case 'V':
136-
alvik.disablePositionControlRight();
137-
alvik.setRpmRight(value);
138-
break;
139-
case 'P':
140-
alvik.setPositionRight(value);
141-
ack_required=MOVEMENT_RIGHT;
142-
ack_check=true;
143-
break;
144-
case 'Z':
145-
alvik.resetPositionRight(value);
146-
break;
134+
if (label=='R'){
135+
switch (control_type){
136+
case 'V':
137+
alvik.disablePositionControlRight();
138+
alvik.setRpmRight(value);
139+
break;
140+
case 'P':
141+
alvik.setPositionRight(value);
142+
ack_required=MOVEMENT_RIGHT;
143+
ack_check=true;
144+
break;
145+
case 'Z':
146+
alvik.resetPositionRight(value);
147+
break;
148+
}
147149
}
148-
}
149-
break;
150-
151-
152-
case 'A':
153-
packeter.unpacketC2F(code,position_left, position_right);
154-
alvik.disableKinematicsMovement();
155-
alvik.setPosition(position_left, position_right);
156-
ack_required=MOVEMENT_POSITION;
157-
ack_check=true;
158-
break;
159-
160-
161-
case 'S':
162-
packeter.unpacketC2B(code,servo_A,servo_B);
163-
alvik.setServoA(servo_A);
164-
alvik.setServoB(servo_B);
165-
break;
166-
167-
case 'L':
168-
packeter.unpacketC1B(code,leds);
169-
alvik.setAllLeds(leds);
170-
break;
171-
172-
case 'P':
173-
packeter.unpacketC1B3F(code,pid,kp,ki,kd);
174-
if (pid=='L'){
175-
alvik.setKPidLeft(kp,ki,kd);
176-
}
177-
if (pid=='R'){
178-
alvik.setKPidRight(kp,ki,kd);
179-
}
180-
break;
181-
182-
case 'R':
183-
packeter.unpacketC1F(code, value);
184-
alvik.disablePositionControl();
185-
alvik.rotate(value);
186-
ack_required=MOVEMENT_ROTATE;
187-
ack_check=true;
188-
break;
189-
190-
case 'G':
191-
packeter.unpacketC1F(code, value);
192-
alvik.disablePositionControl();
193-
alvik.move(value);
194-
ack_required=MOVEMENT_MOVE;
195-
ack_check=true;
196-
break;
197-
198-
case 'Z':
199-
packeter.unpacketC3F(code, x, y, theta);
200-
alvik.resetPose(x, y, theta);
201-
break;
202-
150+
break;
151+
152+
153+
case 'A':
154+
packeter.unpacketC2F(code,position_left, position_right);
155+
alvik.disableKinematicsMovement();
156+
alvik.setPosition(position_left, position_right);
157+
ack_required=MOVEMENT_POSITION;
158+
ack_check=true;
159+
break;
160+
161+
162+
case 'S':
163+
packeter.unpacketC2B(code,servo_A,servo_B);
164+
alvik.setServoA(servo_A);
165+
alvik.setServoB(servo_B);
166+
break;
167+
168+
case 'L':
169+
packeter.unpacketC1B(code,leds);
170+
alvik.setAllLeds(leds);
171+
break;
172+
173+
case 'P':
174+
packeter.unpacketC1B3F(code,pid,kp,ki,kd);
175+
if (pid=='L'){
176+
alvik.setKPidLeft(kp,ki,kd);
177+
}
178+
if (pid=='R'){
179+
alvik.setKPidRight(kp,ki,kd);
180+
}
181+
break;
182+
183+
case 'R':
184+
packeter.unpacketC1F(code, value);
185+
alvik.disablePositionControl();
186+
alvik.rotate(value);
187+
ack_required=MOVEMENT_ROTATE;
188+
ack_check=true;
189+
break;
190+
191+
case 'G':
192+
packeter.unpacketC1F(code, value);
193+
alvik.disablePositionControl();
194+
alvik.move(value);
195+
ack_required=MOVEMENT_MOVE;
196+
ack_check=true;
197+
break;
198+
199+
case 'Z':
200+
packeter.unpacketC3F(code, x, y, theta);
201+
alvik.resetPose(x, y, theta);
202+
break;
203+
}
204+
}
205+
switch (code){
203206
case 'X':
204207
packeter.unpacketC1B(code, ack_code);
205208
if (ack_code == 'K') {

‎src/Arduino_AlvikCarrier.cpp

Lines changed: 16 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -298,6 +298,10 @@ float Arduino_AlvikCarrier::isBatteryCharging(){
298298
return charging;
299299
}
300300

301+
bool Arduino_AlvikCarrier::isBatteryAlert(){
302+
return battery_alert;
303+
}
304+
301305

302306

303307
/******************************************************************************************************/
@@ -982,6 +986,7 @@ void Arduino_AlvikCarrier::beginBehaviours(){
982986
first_lift = true;
983987
battery_alert_time = millis();
984988
battery_alert_wave = 100;
989+
battery_alert = false;
985990
}
986991

987992

@@ -1007,7 +1012,7 @@ void Arduino_AlvikCarrier::updateBehaviours(){
10071012

10081013
// battery alert
10091014
if ((1<<(BATTERY_ALERT-1)) & behaviours){
1010-
if (getBatteryVoltage()<BATTERY_ALERT_MINIMUM_VOLTAGE){
1015+
if ((isBatteryCharging()==-1)&&(getBatteryChargePercentage()<BATTERY_ALERT_MINIMUM_CHARGE)){
10111016
if (millis()-battery_alert_time>battery_alert_wave){
10121017
battery_alert_time = millis();
10131018
if (battery_alert_wave==400){
@@ -1018,11 +1023,19 @@ void Arduino_AlvikCarrier::updateBehaviours(){
10181023
setLeds(COLOR_BLACK);
10191024
battery_alert_wave=400;
10201025
}
1026+
}
1027+
if (getBatteryChargePercentage()<BATTERY_ALERT_STOP_CHARGE){
1028+
battery_alert = true;
10211029
setRpm(0,0);
1022-
motor_left->stop();
1023-
motor_right->stop();
1030+
setIlluminator(false);
10241031
}
10251032
}
1033+
else{
1034+
battery_alert = false;
1035+
}
1036+
}
1037+
else{
1038+
battery_alert = false;
10261039
}
10271040

10281041
}

‎src/Arduino_AlvikCarrier.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -96,6 +96,7 @@ class Arduino_AlvikCarrier{
9696
bool first_lift;
9797
unsigned long battery_alert_time;
9898
unsigned long battery_alert_wave;
99+
bool battery_alert;
99100

100101

101102

@@ -159,6 +160,7 @@ class Arduino_AlvikCarrier{
159160
float getBatteryVoltage(); // get Voltage
160161
float getBatteryChargePercentage(); // get battery percentage
161162
float isBatteryCharging(); // return -1 if battery discharge or 1 if charging
163+
bool isBatteryAlert(); // return true if battery is LOW and alert behaviour is setted, otherwise false
162164

163165

164166

@@ -190,7 +192,6 @@ class Arduino_AlvikCarrier{
190192

191193

192194

193-
194195
// Touch
195196
int beginTouch(); // initialize touch
196197
void updateTouch(); // update touch status

‎src/definitions/robot_definitions.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -96,7 +96,8 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
9696
#define VERSION_BYTE_LOW 3
9797

9898
// Battery stats
99-
#define BATTERY_ALERT_MINIMUM_VOLTAGE 4.0
99+
#define BATTERY_ALERT_MINIMUM_CHARGE 96.0
100+
#define BATTERY_ALERT_STOP_CHARGE 94.0
100101

101102

102103

0 commit comments

Comments
 (0)