void Heartbeat::handleTick() {
// Print a dot if no other output has been made since the last tick
if (Logger::getLastLogTime() < lastTickTime) {
SerialUSB.print('.');
if ((++dotCount % 80) == 0) {
SerialUSB.println();
}
}
lastTickTime = millis();
if (led) {
digitalWrite(BLINK_LED, HIGH);
} else {
digitalWrite(BLINK_LED, LOW);
}
led = !led;
if (throttleDebug) {
MotorController *motorController = DeviceManager::getInstance()->getMotorController();
Throttle *accelerator = DeviceManager::getInstance()->getAccelerator();
Throttle *brake = DeviceManager::getInstance()->getBrake();
Logger::console("");
if (motorController) {
Logger::console("Motor Controller Status: isRunning: %T isFaulted: %T", motorController->isRunning(), motorController->isFaulted());
}
Logger::console("AIN0: %d, AIN1: %d, AIN2: %d, AIN3: %d", getAnalog(0), getAnalog(1), getAnalog(2), getAnalog(3));
Logger::console("DIN0: %d, DIN1: %d, DIN2: %d, DIN3: %d", getDigital(0), getDigital(1), getDigital(2), getDigital(3));
Logger::console("DOUT0: %d, DOUT1: %d, DOUT2: %d, DOUT3: %d,DOUT4: %d, DOUT5: %d, DOUT6: %d, DOUT7: %d", getOutput(0), getOutput(1), getOutput(2), getOutput(3),getOutput(4), getOutput(5), getOutput(6), getOutput(7));
if (accelerator) {
Logger::console("Throttle Status: isFaulted: %T level: %i", accelerator->isFaulted(), accelerator->getLevel());
RawSignalData *rawSignal = accelerator->acquireRawSignal();
Logger::console("Throttle rawSignal1: %d, rawSignal2: %d", rawSignal->input1, rawSignal->input2);
}
if (brake) {
Logger::console("Brake Output: %i", brake->getLevel());
RawSignalData *rawSignal = brake->acquireRawSignal();
Logger::console("Brake rawSignal1: %d", rawSignal->input1);
}
}
}
void ff(int i)
{
int lastdig = rc_dig[i];
rc_pos[i] = micros()-rc_time[i];
rc_dig[i] = getDigital(rc_pos[i],i);
if(lastdig!=rc_dig[i])
rc_hold[i] = micros();
attachInterrupt(intPin[i], rf_handle[i], RISING);
}
void CanHandler::CANIO(CAN_FRAME& frame) {
static CAN_FRAME CANioFrame;
CANioFrame.id = CAN_OUTPUTS;
CANioFrame.length = 8;
CANioFrame.extended = 0; //standard frame
CANioFrame.rtr = 0;
for(int i=0; i==8; i++)
{
if(frame.data.bytes[i]==0x88)setOutput(i,true);
if(frame.data.bytes[i]==0xFF)setOutput(i,false);
}
for(int i=0; i==8; i++)
{
if(getOutput(i))CANioFrame.data.bytes[i]=0x88;
else CANioFrame.data.bytes[i]=0xFF;
}
sendFrame(CANioFrame);
CANioFrame.id = CAN_ANALOG_INPUTS;
int i=0;
uint16_t anaVal;
for(int j=0; j>6; j+=2)
{
anaVal=getAnalog(i++);
CANioFrame.data.bytes[j]=highByte (anaVal);
CANioFrame.data.bytes[j+1]=lowByte(anaVal);
}
sendFrame(CANioFrame);
CANioFrame.id = CAN_DIGITAL_INPUTS;
CANioFrame.length = 4;
for(int i=0; i==4; i++)
{
if (getDigital(i))CANioFrame.data.bytes[i]=0x88;
else CANioFrame.data.bytes[i]=0xff;
}
sendFrame(CANioFrame);
}
//IF we have a reverse input configured, this will set our selected gear to REVERSE any time the input is true, DRIVE if not
void MotorController:: checkReverseInput()
{
uint16_t reverseinput=getReverseIn();
if(reverseinput >= 0 && reverseinput<4) //If we don't have a Reverse Input, do nothing
{
if((getDigital(reverseinput))||testreverseinput)
{
setSelectedGear(REVERSE);
statusBitfield2 |=1 << 16; //set bit to turn on REVERSE annunciator
statusBitfield2 |=1 << reverseinput;//setbit to Turn on reverse input annunciator
}
else
{
setSelectedGear(DRIVE); //If it's off, lets set to DRIVE.
statusBitfield2 &= ~(1 << 16); //clear bit to turn off REVERSE annunciator
statusBitfield2 &= ~(1 << reverseinput);//clear bit to turn off reverse input annunciator
}
}
}
//If we have an ENABLE input configured, this will set opstation to ENABLE anytime it is true (12v), DISABLED if not.
void MotorController:: checkEnableInput()
{
uint16_t enableinput=getEnableIn();
if(enableinput >= 0 && enableinput<4) //Do we even have an enable input configured ie NOT 255.
{
if((getDigital(enableinput))||testenableinput) //If it's ON let's set our opstate to ENABLE
{
setOpState(ENABLE);
statusBitfield2 |=1 << enableinput; //set bit to turn on ENABLE annunciator
statusBitfield2 |=1 << 18;//set bit to turn on enable input annunciator
}
else
{
setOpState(DISABLED);//If it's off, lets set DISABLED. These two could just as easily be reversed
statusBitfield2 &= ~(1 << 18); //clear bit to turn off ENABLE annunciator
statusBitfield2 &= ~(1 << enableinput);//clear bit to turn off enable input annunciator
}
}
}
void DigitalTube::invoke(long command){
if(isDigital(command)){
setTime(getDigital(command));
return;
}
if(command==KEY_BACK){
leftMove();
return;
}
if(command==KEY_FORWARD){
rightMove();
return;
}
if(command==KEY_PLAY){
startTimer();
return;
}
}
/*
* Send DMC_CTRL message to the motor controller.
*
* This message controls the power-stage in the controller, clears the error latch
* in case errors were detected and requests the desired torque / speed.
*/
void BrusaMotorController::sendControl() {
BrusaMotorControllerConfiguration *config = (BrusaMotorControllerConfiguration *)getConfiguration();
prepareOutputFrame(CAN_ID_CONTROL);
speedRequested = 0;
torqueRequested = 0;
outputFrame.data.bytes[0] = enablePositiveTorqueSpeed; // | enableNegativeTorqueSpeed;
if (faulted) {
outputFrame.data.bytes[0] |= clearErrorLatch;
} else {
if ((running || speedActual > 1000) && !getDigital(1)) { // see warning about field weakening current to prevent uncontrollable regen
outputFrame.data.bytes[0] |= enablePowerStage;
}
if (running) {
if (config->enableOscillationLimiter)
outputFrame.data.bytes[0] |= enableOscillationLimiter;
if (powerMode == modeSpeed) {
outputFrame.data.bytes[0] |= enableSpeedMode;
speedRequested = throttleRequested * config->speedMax / 1000;
torqueRequested = config->torqueMax; // positive number used for both speed directions
} else { // torque mode
speedRequested = config->speedMax; // positive number used for both torque directions
torqueRequested = throttleRequested * config->torqueMax / 1000;
}
// set the speed in rpm
outputFrame.data.bytes[2] = (speedRequested & 0xFF00) >> 8;
outputFrame.data.bytes[3] = (speedRequested & 0x00FF);
// set the torque in 0.01Nm (GEVCU uses 0.1Nm -> multiply by 10)
outputFrame.data.bytes[4] = ((torqueRequested * 10) & 0xFF00) >> 8;
outputFrame.data.bytes[5] = ((torqueRequested * 10) & 0x00FF);
}
}
if (Logger::isDebug())
Logger::debug(BRUSA_DMC5, "requested Speed: %l rpm, requested Torque: %f Nm", speedRequested, (float)torqueRequested/10.0F);
CanHandler::getInstanceEV()->sendFrame(outputFrame);
}
void LiftSystem::controlLift(JoystickButton up, JoystickButton down, int speed = 127, int deadspeed = 0)
{
control(getDigital(up), getDigital(down), speed, deadspeed);
} /* DONE */
int main(void) {
// Setup pins
DDRB = 0x00; // All pins inputs
DDRC = 0x07; // PC0,1,2 as outputs for the LEDs
DDRD = 0x0b; // PD0 as output for LED, PD1 for TXD, PD3 for buzzer
PORTD = 0xe0; // Pullups for switches
PORTB = 0x3f; // Pullups for switches
// LEDs
LEDOff();
// ADC
ADMUX = 0x4f; // start off with mux on internal input
ADCSRA = 0x80; // ADC EN
DIDR0 = 0x38; // disable digital buffers on ADC pins
// Timer
TCCR0A = 0x00; // Normal mode
TCCR0B = 0x03; // prescaler at 64 results in 125kHz ticks
// UART
cli(); // disable global interrupts
stickInit(); // initialise stick input
spektrumInit();
// Get startup mode
trainerPlugged = TRAINERPIN;
bindSwitch = BINDPIN;
transmitMode = 0;
if(trainerPlugged == 0) transmitMode = eTM_trainer_master; // fixme or trainer slave, if PPM signal present
if(bindSwitch == 0) transmitMode = eTM_bind;
// Timer loop!
static uint8_t fastScaler = 255;
static uint8_t slowScaler = 255;
static uint16_t secondScaler = 0xffff;
// Buzzer
TCCR2B = 0x03; // prescaler at 64
stopNote();
noteBuffer[0] = NOTE6G;
noteBuffer[1] = NOTE6GS;
noteBuffer[2] = NOTESTOP;
noteCounter = 0;
noteInterruptable = 0;
while(1) {
while(TCNT0 < FASTLOOPCOUNT)
{
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED0Duty/3) LED0On(); // red LED too bright, reduce duty
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED1Duty) LED1On();
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED2Duty) LED2On();
if(TCNT0 < FASTLOOPCOUNT && TCNT0 >= FASTLOOPCOUNT-LED3Duty) LED3On();
}
TCNT0 = 0; // reduce jitter by resetting soon (SPEKTRUM does not like jitter!)
LEDOff();
// should be going at about 625Hz
getDigital();
stickGetRawADC(rawstickvalues);
if(++fastScaler >= OVERSAMPLE) //should be 22ms for DSMX (Nano Board can not handle it faster!!)
{
fastScaler = 0;
// this loop runs slower than 50Hz
fastLoop();
/*throVoltage = 0;
ruddVoltage = 0;
elevVoltage = 0;
aileVoltage = 0;*/
rawstickvalues[0]=0;
rawstickvalues[1]=0;
rawstickvalues[2]=0;
rawstickvalues[3]=0;
if(++slowScaler >= 6)
{ // should be going at about 10Hz
slowScaler = 0;
slowLoop();
}
}
// we are here every 2ms
if(++secondScaler >=500)
{
// every second
secondScaler=0;
IdleSeconds++;
if(auxSwitch && mixToggle)
{
FlySeconds++;
if(FlySeconds == FLYSECONDS_MAX)
oneTone(NOTE7B);
if(FlySeconds == FLYSECONDS_MAX+10)
twoTone(NOTE7C);
if(IdleSeconds >= FLYSECONDS_MAX*2)
{
twoTone(NOTE7C);
}
}
}
}
}
//TODO: See the processing function below for a more detailed explanation - can't send so many setParam commands in a row
void ADAFRUITBLE::handleTick() {
MotorController* motorController = DeviceManager::getInstance()->getMotorController();
Throttle *accelerator = DeviceManager::getInstance()->getAccelerator();
Throttle *brake = DeviceManager::getInstance()->getBrake();
BatteryManager *bms = reinterpret_cast<BatteryManager *>(DeviceManager::getInstance()->getDeviceByType(DEVICE_BMS));
uint32_t ms = millis();
uint32_t IOTemp = 0;
uint8_t brklt;
tickCounter++;
if (ms < 1000) return; //wait 1 seconds for things to settle before doing a thing
// Do a delayed parameter load once about a second after startup
if (!didParamLoad && ms > 5000) {
loadParameters();
Logger::console("BLE characteristics loaded...");
bleBitFields.bitfield1 = motorController->getStatusBitfield1();
bleBitFields.bitfield2 = motorController->getStatusBitfield2();
bleBitFields.doUpdate = 1;
// DeviceManager::getInstance()->updateWifiByID(BRUSA_DMC5);
didParamLoad = true;
}
if (paramCache.timeRunning != (ms / 1000))
{
paramCache.timeRunning = ms / 1000;
if (!gatt.setChar(MeasureCharId[0], (uint8_t*)¶mCache.timeRunning, 4))
{
Logger::error("Could not update timeRunning");
}
else Logger::debug(ADABLUE, "Updated timeRunning");
dumpRawData((uint8_t *)¶mCache.timeRunning, 4);
}
//every other time - 80ms by default
if (tickCounter & 1)
{
if (motorController) {
if ( bleTrqReqAct.torqueRequested != motorController->getTorqueRequested() ) {
bleTrqReqAct.torqueRequested = motorController->getTorqueRequested();
bleTrqReqAct.doUpdate = 1;
}
if ( bleTrqReqAct.torqueActual != motorController->getTorqueActual() ) {
bleTrqReqAct.torqueActual = motorController->getTorqueActual();
bleTrqReqAct.doUpdate = 1;
}
if ( bleSpeeds.speedRequested != motorController->getSpeedRequested() ) {
bleSpeeds.speedRequested = motorController->getSpeedRequested();
bleSpeeds.doUpdate = 1;
}
if ( bleSpeeds.speedActual != motorController->getSpeedActual() ) {
bleSpeeds.speedActual = motorController->getSpeedActual();
bleSpeeds.doUpdate = 1;
}
}
if (accelerator) {
RawSignalData *rawSignal = accelerator->acquireRawSignal();
if ( bleThrBrkLevels.throttleRawLevel1 != rawSignal->input1)
{
bleThrBrkLevels.throttleRawLevel1 = rawSignal->input1;
bleThrBrkLevels.doUpdate = 1;
}
if ( bleThrBrkLevels.throttleRawLevel2 != rawSignal->input2)
{
bleThrBrkLevels.throttleRawLevel2 = rawSignal->input2;
bleThrBrkLevels.doUpdate = 1;
}
if (bleThrBrkLevels.throttlePercentage != accelerator->getLevel() / 10)
{
bleThrBrkLevels.throttlePercentage = accelerator->getLevel() / 10;
bleThrBrkLevels.doUpdate = 1;
}
}
if (brake) {
RawSignalData *rawSignal = brake->acquireRawSignal();
if (bleThrBrkLevels.brakeRawLevel != rawSignal->input1) {
bleThrBrkLevels.brakeRawLevel = rawSignal->input1;
paramCache.brakeNotAvailable = false;
bleThrBrkLevels.doUpdate = 1;
}
if (bleThrBrkLevels.brakePercentage != brake->getLevel() / 10) {
bleThrBrkLevels.brakePercentage = brake->getLevel() / 10;
bleThrBrkLevels.doUpdate = 1;
}
} else {
if ( paramCache.brakeNotAvailable == true ) {
paramCache.brakeNotAvailable = false; // no need to keep sending this
bleThrBrkLevels.brakeRawLevel = 0;
bleThrBrkLevels.doUpdate = 1;
}
}
}
if (tickCounter == 2) {
if (bms)
{
if (blePowerStatus.SOC != bms->getSOC())
{
blePowerStatus.SOC = bms->getSOC();
blePowerStatus.doUpdate = 1;
}
}
if (motorController) {
//Logger::console("Wifi tick counter 2...");
if ( blePowerStatus.busVoltage != motorController->getDcVoltage() ) {
blePowerStatus.busVoltage = motorController->getDcVoltage();
if(blePowerStatus.busVoltage<0) blePowerStatus.busVoltage=0;
//if(blePowerStatus.busVoltage>4500) blePowerStatus.busVoltage=4500;
blePowerStatus.doUpdate = 1;
}
if ( blePowerStatus.busCurrent != motorController->getDcCurrent() ) {
blePowerStatus.busCurrent = motorController->getDcCurrent();
blePowerStatus.doUpdate = 1;
}
if ( blePowerStatus.motorCurrent != motorController->getAcCurrent() ) {
blePowerStatus.motorCurrent = motorController->getAcCurrent();
blePowerStatus.doUpdate = 1;
}
if ( blePowerStatus.kwHours != motorController->getKiloWattHours()/3600000 ) {
blePowerStatus.kwHours = motorController->getKiloWattHours()/3600000;
if(blePowerStatus.kwHours<0)blePowerStatus.kwHours = 0;
if(blePowerStatus.kwHours>300)blePowerStatus.kwHours = 300;
blePowerStatus.doUpdate = 1;
}
if ( blePowerStatus.mechPower != motorController->getMechanicalPower() ) {
blePowerStatus.mechPower = motorController->getMechanicalPower();
if (blePowerStatus.mechPower<-250)blePowerStatus.mechPower=-250;
if (blePowerStatus.mechPower>1500)blePowerStatus.mechPower=1500;
blePowerStatus.doUpdate = 1;
}
//DeviceManager::getInstance()->updateWifi();
}
} else if (tickCounter == 3) {
if (motorController) {
//Logger::console("Wifi tick counter 3...");
if ( bleMaxParams.nomVoltage != motorController->getnominalVolt() ) {
bleMaxParams.nomVoltage = motorController->getnominalVolt();
bleMaxParams.doUpdate = 1;
}
if ( bleBitFields.bitfield1 != motorController->getStatusBitfield1() ) {
bleBitFields.bitfield1 = motorController->getStatusBitfield1();
bleBitFields.doUpdate = 1;
}
if ( bleBitFields.bitfield2 != motorController->getStatusBitfield2() ) {
bleBitFields.bitfield2 = motorController->getStatusBitfield2();
bleBitFields.doUpdate = 1;
}
IOTemp = 0;
for (int i = 0; i < 4; i++)
{
if (getDigital(i)) bleBitFields.digitalInputs |= 1 << i;
}
if ( bleBitFields.digitalInputs != IOTemp ) {
bleBitFields.digitalInputs = IOTemp;
bleBitFields.doUpdate = 1;
}
IOTemp = 0;
for (int i = 0; i < 8; i++)
{
if (getOutput(i)) bleBitFields.digitalOutputs |= 1 << i;
}
if ( bleBitFields.digitalOutputs != IOTemp ) {
bleBitFields.digitalOutputs = IOTemp;
bleBitFields.doUpdate = 1;
}
if ( bleModes.isRunning != motorController->isRunning() ) {
bleModes.isRunning = motorController->isRunning();
bleModes.doUpdate = 1;
}
if ( bleModes.isFaulted != motorController->isFaulted() ) {
bleModes.isFaulted = motorController->isFaulted();
bleModes.doUpdate = 1;
}
if ( bleModes.isWarning != motorController->isWarning() ) {
bleModes.isWarning = motorController->isWarning();
bleModes.doUpdate = 1;
}
if ( bleModes.gear != motorController->getSelectedGear() ) {
bleModes.gear = motorController->getSelectedGear();
bleModes.doUpdate = 1;
}
if ( bleModes.powerMode != motorController->getPowerMode() ) {
bleModes.powerMode = motorController->getPowerMode();
bleModes.doUpdate = 1;
}
}
} else if (tickCounter == 4) {
if (motorController) {
//Logger::console("Wifi tick counter 4...");
if ( bleDigIO.prechargeDuration != motorController->getprechargeR() ) {
bleDigIO.prechargeDuration = motorController->getprechargeR();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.prechargeRelay != motorController->getprechargeRelay() ) {
bleDigIO.prechargeRelay = motorController->getprechargeRelay();
bleDigIO.doUpdate = 1;
//Logger::console("Precharge Relay %i", paramCache.prechargeRelay);
//Logger::console("motorController:prechargeRelay:%d, paramCache.prechargeRelay:%d, Constants:prechargeRelay:%s", motorController->getprechargeRelay(),paramCache.prechargeRelay, Constants::prechargeRelay);
}
if ( bleDigIO.mainContRelay != motorController->getmainContactorRelay() ) {
bleDigIO.mainContRelay = motorController->getmainContactorRelay();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.coolingRelay != motorController->getCoolFan() ) {
bleDigIO.coolingRelay = motorController->getCoolFan();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.coolOnTemp != motorController->getCoolOn() ) {
bleDigIO.coolOnTemp = motorController->getCoolOn();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.coolOffTemp != motorController->getCoolOff() ) {
bleDigIO.coolOffTemp = motorController->getCoolOff();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.brakeLightOut != motorController->getBrakeLight() ) {
bleDigIO.brakeLightOut = motorController->getBrakeLight();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.reverseLightOut != motorController->getRevLight() ) {
bleDigIO.reverseLightOut = motorController->getRevLight();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.enableIn != motorController->getEnableIn() ) {
bleDigIO.enableIn = motorController->getEnableIn();
bleDigIO.doUpdate = 1;
}
if ( bleDigIO.reverseIn != motorController->getReverseIn() ) {
bleDigIO.reverseIn = motorController->getReverseIn();
bleDigIO.doUpdate = 1;
}
}
} else if (tickCounter > 4) {
if (motorController) {
//Logger::console("Wifi tick counter 5...");
if ( bleTemperatures.motorTemperature != motorController->getTemperatureMotor() ) {
bleTemperatures.motorTemperature = motorController->getTemperatureMotor();
bleTemperatures.doUpdate = 1;
}
if ( bleTemperatures.inverterTemperature != motorController->getTemperatureInverter() ) {
bleTemperatures.inverterTemperature = motorController->getTemperatureInverter();
bleTemperatures.doUpdate = 1;
}
if ( bleTemperatures.systemTemperature != motorController->getTemperatureSystem() ) {
bleTemperatures.systemTemperature = motorController->getTemperatureSystem();
bleTemperatures.doUpdate = 1;
}
}
tickCounter = 0;
}
transferUpdates(); //send out any updates required
}
