void SendSensorData(void)
{
uint16_t tmp = 0;
generic_message_t* radioMessage;
// TODO: we should be wrapping this data in MAC layer data in the radio task, so
// we shouldn't be allocating the full packet size here
// TODO: do something clever if we ran out of RAM
radioMessage = pvPortMalloc(RADIO_MAX_PACKET_LENGTH);
radioMessage->cmd = SENSOR_MSG;
// moist 0
tmp = Float_To_SMS(sensorData.moist0);
radioMessage->payload.sensor_message.moisture0 = tmp;
// moist 1
tmp = Float_To_SMS(sensorData.moist1);
radioMessage->payload.sensor_message.moisture1 = tmp;
// moist 2
tmp = Float_To_SMS(sensorData.moist2);
radioMessage->payload.sensor_message.moisture2 = tmp;
// humid
tmp = Float_To_HTU21D_Humid(sensorData.humid);
radioMessage->payload.sensor_message.humid = tmp;
// temp 0
radioMessage->payload.sensor_message.temp0 = sensorData.temp0;
// temp 1
radioMessage->payload.sensor_message.temp1 = sensorData.temp1;
// temp 2
radioMessage->payload.sensor_message.temp2 = sensorData.temp2;
// air temp
tmp = Float_To_HTU21D_Temp(sensorData.tempAir);
radioMessage->payload.sensor_message.air_temp = tmp;
// battery level
radioMessage->payload.sensor_message.battery_level = GetBatteryVoltage();
// Chip temperature
radioMessage->payload.sensor_message.chip_temp = GetChipTemperature();
// Sensor altitude
radioMessage->payload.sensor_message.alt = sensorData.alt;
// Sensor acceleration
radioMessage->payload.sensor_message.acc = sensorData.acc;
DEBUG("(SENSORS_TASK) Sending sensor message to radio task\r\n");
SendToBaseStation((uint8_t*)radioMessage, sizeof(generic_message_t));
}
/**
* Task to handle the display of LEDs
*/
void LEDs::LEDRunner() {
// This is kind of ugly :(
auto ds = DriverStation::GetInstance();
int mode = 0;
//float hueOutput = 0;
auto leds = LEDController::GetInstance();
leds->SetBrightness(30);
LEDStrip frontLeft(0, 8);
LEDStrip rearLeft(8, 16);
LEDStrip rearRight(24, 16);
LEDStrip frontRight(40, 8);
std::vector<Util::Color> tmp;
while (taskRunning.test_and_set()) {
Wait(0.2);
if (ds->IsDisabled()) {
tmp.clear(); tmp.resize(16, 0xffff00);
if (!ds->IsDSAttached()) {
// Not attached to the DS, so, alternate yellow/off
for (int i = 0; i < 8; i++) {
tmp[i * 2] = 0x000000;
}
}
rearLeft.Set(tmp);
rearRight.Set(tmp);
tmp.resize(8);
frontRight.Set(tmp);
frontLeft.Set(tmp);
mode = Auton::GetInstance()->GetMode();
tmp.clear();
tmp.resize(3);
tmp[0] = mode & 1 ? 0xffff00 : 0x00;
tmp[1] = mode & 2 ? 0xffff00 : 0x00;
tmp[2] = mode & 4 ? 0xffff00 : 0x00;
rearRight.Set(tmp, 0);
tmp.clear(); tmp.resize(6);
tmp[0] = ds->IsDSAttached() ? 0x00ff00 : 0x00;
tmp[1] = ds->IsFMSAttached() ? 0x00ff00 : 0x00;
float hueOutput = Interpolate(ds->GetBatteryVoltage(), 12.0f, 13.0f, 0.0f, 120.0f);
//hueOutput = ((int)hueOutput + 5) % 360;
//float hueOutput
//printf("Voltage: %f (%f) ", ds->GetBatteryVoltage(), hueOutput);
tmp[2] = Util::Color(hueOutput, 1.0, 0.75);
//Util::RGB c = tmp[2];
//printf("(%d, %d, %d)\n", c.r, c.g, c.b);
auto position = ds->GetLocation();
auto alliance = ds->GetAlliance();
Util::Color allianceColor = alliance == DriverStation::kRed ? 0xff0000 : (alliance == DriverStation::kBlue ? 0x0000ff : 0xffff00);
if (position) {
tmp[2 + position] = allianceColor;
//} else {
//position += 1;
//tmp[2 + position] = allianceColor;
//tmp[2 + position + 1] = allianceColor;
//tmp[2 + position + 2] = allianceColor;
}
rearRight.Set(tmp, 3);
rearRight.Show();
} else {
if (ds->IsAutonomous()) {
auto alliance = ds->GetAlliance();
Util::Color allianceColor = alliance == DriverStation::kRed ? 0xff0000 : (alliance == DriverStation::kBlue ? 0x0000ff : 0xffff00);
tmp.clear();
tmp.resize(16, allianceColor);
rearLeft.Set(tmp);
rearRight.Set(tmp);
tmp.resize(8);
frontLeft.Set(tmp);
frontRight.Set(tmp);
} else if (ds->IsOperatorControl()) {
double timeLeft = ds->GetMatchTime();
tmp.clear();
tmp.resize(16, 0xffff00);
if (timeLeft < 0 || timeLeft > 30) {
// All yellow
} else if (timeLeft > 20) {
for (int i = 0; i < 8; i++) {
tmp[i * 2] = 0x00ff00;
}
} else if (timeLeft > 5) {
// off and green->red
float hueOutput = Interpolate((float)timeLeft, 5.0f, 20.f, 0.0f, 120.0f);
for (int i = 0; i < 8; i++) {
tmp[i * 2 + 1] = 0x00;
tmp[i * 2] = Util::Color(hueOutput, 1.0, 0.75);
//tmp[i * 2] = 0x00ff00;
}
} else {
// Just red
tmp.clear();
tmp.resize(16, 0xff0000);
}
if (Grabber::GetInstance()->Get(Grabber::kMini)) {
tmp[0] = 0xff00ff;
tmp[15] = 0xff00ff;
}
if (!Grabber::GetInstance()->Get(Grabber::kMain)) {
tmp[1] = 0x00ffff;
tmp[14] = 0x00ffff;
}
rearLeft.Set(tmp);
rearRight.Set(tmp);
tmp.resize(8);
if (Grabber::GetInstance()->Get(Grabber::kMini)) {
tmp[7] = 0xff00ff;
}
if (!Grabber::GetInstance()->Get(Grabber::kMain)) {
tmp[6] = 0x00ffff;
}
frontLeft.Set(tmp);
frontRight.Set(tmp);
//printf("Match Time: %f\n", ds->GetMatchTime());
}
}
frontLeft.Show();
rearLeft.Show();
frontRight.Show();
rearRight.Show();
}
}
int EventProcessing(){
int t_current_ch1=0;
int t_current_ch2=0;
char t_time_ch1 = 5;
char t_time_ch2 = 5;
char t_anode_ch1 =1;
char t_anode_ch2 =1;
char t_status_ch1 =0;
char t_status_ch2 =0;
uint8_t tx[32];
static char bat=0;
int tm;
static char old_status_ch1=0;
static char old_status_ch2=0;
int temp;
//sprintf(tx, "Test message");
// HAL_UART_Transmit(&huart4, tx, 11, 10);
//return 0;
if(ready) {
t_status_ch1 = RxData[1];
t_anode_ch1 = RxData[0];
t_time_ch1 = RxData[2];
t_current_ch1 = RxData[3];
t_current_ch1 = t_current_ch1<<8;
t_current_ch1 |= RxData[4];
t_status_ch2 = RxData[6];
t_anode_ch2 = RxData[5];
t_time_ch2 = RxData[7];
t_current_ch2 = RxData[8];
t_current_ch2 = t_current_ch2<<8;
t_current_ch2 |= RxData[9];
//////////////////////////////////////////////
_status_ch1=t_status_ch1;
_current_ch1=t_current_ch1;
if((!old_status_ch1)&&(t_status_ch1)){
_anode_ch1=t_anode_ch1;
_status_ch1=t_status_ch1;
_current_ch1=t_current_ch1;
_time_ch1=t_time_ch1;
end_ch1=0;
old_status_ch1 = 1;
SetTimerCH1(1*t_time_ch1);
StartTimerCH1();
}
if((old_status_ch1)&&(!t_status_ch1)) {
old_status_ch1 = 0;
//ResetTimerCH1();
}
/////////////////////////////////////////////
_status_ch2=t_status_ch2;
_current_ch2=t_current_ch2;
if((!old_status_ch2)&&(t_status_ch2)){
_anode_ch2=t_anode_ch2;
_status_ch2=t_status_ch2;
_current_ch2=t_current_ch2;
_time_ch2=t_time_ch2;
end_ch2=0;
old_status_ch2 = 1;
SetTimerCH2(1*t_time_ch2);
StartTimerCH2();
}
if((old_status_ch2)&&(!t_status_ch2)) {
old_status_ch2 = 0;
//ResetTimerCH1();
}
tm = GetTimerCH1();
tx[0] = (char)tm;
tm = GetTimerCH2();
tx[3] = (char)tm;
tx[1] = (char)(GetADC1()/10);//Voltage1_1
tx[2] = (char)(GetADC11()/10);
tx[4] = (char)(GetADC2()/10);
tx[5] = (char)(GetADC21()/10);
tx[6] = (char)(GetBatteryVoltage()*10);//Battery
tx[7] = end_ch1;//end procedure1
tx[8] = end_ch2;//end procedure1
bat ^=1;
tx[9] = '\n';
tx[10] = '\r';
HAL_UART_Transmit(&huart4, tx, 11, 10);
ready = 0;
}
ON_LineControlCH1();
ON_LineControlCH2();
return 1;
}
/**
* Copy status data from the DS task for the user.
* This is used primarily to set digital outputs on the DS.
*/
void DriverStation::SetData()
{
setStatusData(GetBatteryVoltage(), m_digitalOut, m_userStatus,
USER_STATUS_DATA_SIZE, WAIT_FOREVER);
}
