//done
void AnalogTask (void* pvParameters) {
int AnaX = 0;
int AnaY = 0;
for(;;) {
AnaY = 2048-getAnalog(0);
AnaX = getAnalog(1)-2048;
anaAng = atan2f(AnaY,AnaX) - (M_PI/2.00);
anaMag = sqrt(AnaX*AnaX + AnaY*AnaY);
vTaskDelay(20);
}
}
/*********************************************************************************************************
** Function name: getBatVol
** Descriptions: get voltage of battery
*********************************************************************************************************/
float xadow::getBatVol()
{
float vol = 0;
vol = getAnalog(PINBAT);
vol = vol/1023.0*3.3*2;
return vol;
}
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 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);
}
/*
* Retrieve raw input signals from the brake hardware.
*/
RawSignalData *PotBrake::acquireRawSignal() {
PotBrakeConfiguration *config = (PotBrakeConfiguration *) getConfiguration();
sys_io_adc_poll();
rawSignal.input1 = getAnalog(config->AdcPin1);
return &rawSignal;
}
