//% help=pins/servo-write-pin weight=20
//% blockId=device_set_servo_pin block="servo write|pin %name|to %value" blockGap=8
//% parts=microservo trackArgs=0
//% value.min=0 value.max=180
//% name.fieldEditor="gridpicker" name.fieldOptions.columns=4
//% name.fieldOptions.tooltips="false"
void servoWritePin(AnalogPin name, int value) {
PINOP(setServoValue(value));
}
/* set all Servos to next 4 Channels ----------------------------------------*/
void setAllServos(vu16 * receiverChannel, volatile float * copterAngle)
{
s16 servoVals[4];
servoVals[0] = (receiverChannel[parameter[PARA_SERVO0_CH]] == 0) ? 1500 : receiverChannel[parameter[PARA_SERVO0_CH]];
servoVals[1] = (receiverChannel[parameter[PARA_SERVO1_CH]] == 0) ? 1500 : receiverChannel[parameter[PARA_SERVO1_CH]];
servoVals[2] = (receiverChannel[parameter[PARA_SERVO2_CH]] == 0) ? 1500 : receiverChannel[parameter[PARA_SERVO2_CH]];
servoVals[3] = (receiverChannel[parameter[PARA_SERVO3_CH]] == 0) ? 1500 : receiverChannel[parameter[PARA_SERVO3_CH]];
if (parameter[PARA_CAM_X] != 0)
{
if (parameter[PARA_CAM_X] < 10000) // servo 0
{
if (parameter[PARA_CAM_X] < 1000) //no invert
{
servoVals[0] += (s16) ((copterAngle[X] - 90.0) * (parameter[PARA_CAM_X] / 90.0));
}
else // invert
{
servoVals[0] = (s16) 3000 - (((copterAngle[X] - 90.0) * ((parameter[PARA_CAM_X] - 1000) / 90.0)) + servoVals[0]);
}
}
else if (parameter[PARA_CAM_X] < 20000) // servo 1
{
if ((parameter[PARA_CAM_X] - 10000) < 1000) //no invert
{
servoVals[1] += (s16) ((copterAngle[X] - 90.0) * ((parameter[PARA_CAM_X] - 10000) / 90.0));
}
else // invert
{
servoVals[1] = (s16) 3000 - (((copterAngle[X] - 90.0) * ((parameter[PARA_CAM_X] - 11000) / 90.0)) + servoVals[1]);
}
}
else if (parameter[PARA_CAM_X] < 30000) // servo 2
{
if ((parameter[PARA_CAM_X] - 20000) < 1000) //no invert
{
servoVals[2] += (s16) ((copterAngle[X] - 90.0) * ((parameter[PARA_CAM_X] - 20000) / 90.0));
}
else // invert
{
servoVals[2] = (s16) 3000 - (((copterAngle[X] - 90.0) * ((parameter[PARA_CAM_X] - 21000) / 90.0)) + servoVals[2]);
}
}
else if (parameter[PARA_CAM_X] < 40000) // servo 3
{
if ((parameter[PARA_CAM_X] - 30000) < 1000) //no invert
{
servoVals[3] += (s16) ((copterAngle[X] - 90.0) * ((parameter[PARA_CAM_X] - 30000) / 90.0));
}
else // invert
{
servoVals[3] = (s16) 3000 - (((copterAngle[X] - 90.0) * ((parameter[PARA_CAM_X] - 31000) / 90.0)) + servoVals[3]);
}
}
}
if (parameter[PARA_CAM_Y] != 0)
{
if (parameter[PARA_CAM_Y] < 10000) // servo 0
{
if (parameter[PARA_CAM_Y] < 1000) //no invert
{
servoVals[0] += (s16) ((copterAngle[Y] - 90.0) * (parameter[PARA_CAM_Y] / 90.0));
}
else // invert
{
servoVals[0] = (s16) 3000 - (((copterAngle[Y] - 90.0) * ((parameter[PARA_CAM_Y] - 1000) / 90.0)) + servoVals[0]);
}
}
else if (parameter[PARA_CAM_Y] < 20000) // servo 1
{
if ((parameter[PARA_CAM_Y] - 10000) < 1000) //no invert
{
servoVals[1] += (s16) ((copterAngle[Y] - 90.0) * ((parameter[PARA_CAM_Y] - 10000) / 90.0));
}
else // invert
{
servoVals[1] = (s16) 3000 - (((copterAngle[Y] - 90.0) * ((parameter[PARA_CAM_Y] - 11000) / 90.0)) + servoVals[1]);
}
}
else if (parameter[PARA_CAM_Y] < 30000) // servo 2
{
if ((parameter[PARA_CAM_Y] - 20000) < 1000) //no invert
{
servoVals[2] += (s16) ((copterAngle[Y] - 90.0) * ((parameter[PARA_CAM_Y] - 20000) / 90.0));
}
else // invert
{
servoVals[2] = (s16) 3000 - (((copterAngle[Y] - 90.0) * ((parameter[PARA_CAM_Y] - 21000) / 90.0)) + servoVals[2]);
}
}
else if (parameter[PARA_CAM_Y] < 40000) // servo 3
{
if ((parameter[PARA_CAM_Y] - 30000) < 1000) //no invert
{
servoVals[3] += (s16) ((copterAngle[Y] - 90.0) * ((parameter[PARA_CAM_Y] - 30000) / 90.0));
}
else // invert
{
servoVals[3] = (s16) 3000 - (((copterAngle[Y] - 90.0) * ((parameter[PARA_CAM_Y] - 31000) / 90.0)) + servoVals[3]);
}
}
}
setServoValue(0, servoVals[0]);
setServoValue(1, servoVals[1]);
setServoValue(2, servoVals[2]);
setServoValue(3, servoVals[3]);
}
