void taskUpdateBattery(void)
{
static uint32_t vbatLastServiced = 0;
static uint32_t ibatLastServiced = 0;
if (feature(FEATURE_VBAT)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
voltageMeterUpdate();
batteryUpdate();
}
}
if (feature(FEATURE_AMPERAGE_METER)) {
int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
if (batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC) {
amperageUpdateMeter(ibatTimeSinceLastServiced);
} else {
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle);
bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP));
int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
amperageUpdateVirtualMeter(ibatTimeSinceLastServiced, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset);
}
}
}
}
void init(){
bDisplayDiagnostics=false;
// StartTask(batteryTask);
HTSPBsetupIO(HTPB,0b11110000);
batteryUpdate(HTPB);
nMotorEncoder[grab]=0;
servo[bucket]=81;
}
void init(){
HTSPBsetupIO(HTPB,0b11000000);
//light up for battery
batteryUpdate(HTPB);
servo[bucket]=120;
ubyte in=HTSPBreadIO(HTPB,0b00001111);//get autonomous settings!
bDisplayDiagnostics=false;
nMotorEncoder[back]=0;
nMotorEncoder[grab]=0;
nMotorEncoder[motorC]=0;
int sum=0;
for(int i=0;i<5;i++){
sum+=readAnalogInput(HTPB,1);
wait1Msec(100);
}
stationaryVoltage=sum/5;
while(nNxtButtonPressed!=3){
if(HTSPBreadIO(HTPB,0b00000100)>0)STARTINGPOS=1;
nxtDisplayCenteredBigTextLine(4,"%d",STARTINGPOS);
}
nxtDisplayCenteredBigTextLine(0,"WAITING");
}
void RosComunication::fromBatteryCallback(const std_msgs::Int8& message)
{
emit batteryUpdate(message.data);
}
