void Mobile::sendEBUTwo() {
printf("EBU2\n");
AnalogOut analogTwo;
DigitalOut digitalTwo; //Not really used for now
DigitalIn digitaldummy;
AnalogIn analogdummy;
SimPack tempState; //Locking over methods in other objects might cause problem, this is safer.
logVerbose("Mobile -> sendEBUTro: starting.");
while(not pleased){
m_State.lock();
tempState = state;
m_State.unlock();
try{
et.setEbuTwo(&tempState, &analogTwo, &digitalTwo);
digitaldummy = em.recvDigitalEBUTwo();
analogdummy = em.recvAnalogEBUTwo();
em.sendDigitalCommand(digitalTwo.getChannel(), digitalTwo.getDestination());
em.sendAnalogCommand(analogTwo.getChannel(), analogTwo.getDestination());
}catch(int e){
logError(strerror(errno));
logError("Fatal: Mobile -> sendEBUTwo: could not send any data to EBU 2.");
exit(1);
//HERE if it fails somehow and cannot send to the EBUs it should tell the watchdog to stop all
//operations
}
}
}
/* The method ebuSend locks the packetBuffer and takes out One packet, sends it to the ebu with
* the ebuManager. This method is designed to be started as a thread.
* The data it sends comes from a state which is set in socketReceive() if it is different than the existing one
*/
void Mobile::sendEBUOne() {
printf("EBU1\n");
AnalogOut analogOne;
DigitalOut digitalOne;
DigitalIn digitaldummy;
AnalogIn analogdummy;
SimPack tempState; //Locking over methods in other objects might cause problem, this is safer.
while(not pleased){
m_State.lock();
tempState = state;
m_State.unlock();
try{
et.setEbuOne(&tempState, &analogOne, &digitalOne); //Use EBUTranslator (et) to translate simdata
digitaldummy = em.recvDigitalEBUOne();
analogdummy = em.recvAnalogEBUOne();
em.sendDigitalCommand(digitalOne.getChannel(), digitalOne.getDestination());
em.sendAnalogCommand(analogOne.getChannel(), analogOne.getDestination());
}catch(int e){
perror("sendEBUOne error");
logError(strerror(errno));
throw e;
//HERE if it fails somehow and cannot send to the EBUs it should tell the watchdog to stop all
//operations
}
}
}
mrb_value mrb_digital_out_write(mrb_state* mrb, mrb_value self) {
DigitalOut* obj = static_cast<DigitalOut*>(DATA_PTR(self));
int value;
mrb_get_args(mrb, "i", &value);
obj->write(value);
return mrb_nil_value();
}
int main()
{
DigitalIn<Input> in;
DigitalOut<Output> out;
while (1)
out.set(in.get());
return 0;
}
void WrittenHandler(const GattWriteCallbackParams *Handler)
{
uint8_t buf[TXRX_BUF_LEN];
uint16_t bytesRead;
if (Handler->handle == txCharacteristic.getValueAttribute().getHandle())
{
ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead);
memset(txPayload, 0, TXRX_BUF_LEN);
memcpy(txPayload, buf, TXRX_BUF_LEN);
if(bytesRead>=3)
{
Motor1.write(txPayload[0]>100?0:1);
Motor2.write(txPayload[1]>100?0:1);
Led1.write(txPayload[2]>100?0:1);
Led2.write(txPayload[2]>100?0:1);
}
}
}
void setup()
{
WDT::clear();
nu32.setup();
power_relay.setup();
array_relay.setup();
ground_relay.setup();
motor_relay.setup();
bypass_button.setup();
common_can.setup();
common_can.setup_mask(CAN_FILTER_MASK0, 0x7ff);
common_can.setup_filter(CAN_FILTER0, Can::Addr::os::user_cmds::_id);
common_can_in.setup_rx();
common_can_in.link_filter(CAN_FILTER0, CAN_FILTER_MASK0);
common_can_out.setup_tx(CAN_HIGH_MEDIUM_PRIORITY);
common_can_err.setup_tx(CAN_LOWEST_PRIORITY);
// lcd1.setup();
// lcd2.setup();
voltage_sensor.setup();
current_sensor.setup();
// Initialize sensors,
this->read_ins();
}
void positionPID() {
angularPos = angularRes*encoderCount;
position = angularPos*spoolRadius;
error = setPoint - position;
control = Kp*error + Kd*(error-lastError);
lastError = error;
lastPosition = position;
if ((position > 22)||(position < -12)) {
_enable.write(0.00f);
} else {
if (control >= 0.00f) {
if (control > 1.00f) {
pwmControl = 1.00f;
} else {
pwmControl = control;
}
_phase.write(1);
if (pwmControl > 0.00f) {
_enable.write(pwmControl);
} else {
_enable.write(0.00f);
}
} else {
if (fabs(control) > 1.00f) {
pwmControl = 1.00f;
} else {
pwmControl = fabs(control);
}
_phase.write(0);
if (pwmControl > 0.00f) {
_enable.write(pwmControl);
} else {
_enable.write(0.00f);
}
}
}
}
mrb_value mrb_digital_out_read(mrb_state* mrb, mrb_value self) {
DigitalOut* obj = static_cast<DigitalOut*>(DATA_PTR(self));
int value;
value = obj->read();
return mrb_fixnum_value(value);
}
