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();
}
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 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);
}
}
}
}