//////////// Demo 4. More complicated example: Pitch gimbal by 40 degrees
// with the full control of speed and angle.
// - send control command with the fixed frame rate
// - angle is calculated by the integration of the speed
void example4(SBGC_Parser sbgc_parser) {
SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 };
float speed = 0, angle = 0;
c.mode = SBGC_CONTROL_MODE_SPEED_ANGLE;
// acceleration phase
while (angle < 20.0f) {
speed += 0.5f;
c.speedPITCH = speed * SBGC_SPEED_SCALE;
angle += speed * SBGC_CMD_DELAY / 1000.0f; // degree/sec -> degree/ms
c.anglePITCH = SBGC_DEGREE_TO_ANGLE(angle);
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(SBGC_CMD_DELAY);
}
// de-acceleration phase
while (angle < 40.0f && speed > 0.0f) {
speed -= 0.5f;
c.speedPITCH = speed * SBGC_SPEED_SCALE;
angle += speed * SBGC_CMD_DELAY / 1000.0f;
c.anglePITCH = SBGC_DEGREE_TO_ANGLE(angle);
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(SBGC_CMD_DELAY);
}
}
/////////////////// Demo 1. PITCH and YAW gimbal by 40 and 30 degrees both sides and return back.
// Actual speed depends on PID setting.
// Whait 5 sec to finish
void example1(SBGC_Parser sbgc_parser)
{
SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 };
c.mode = SBGC_CONTROL_MODE_ANGLE;
c.speedROLL = c.speedPITCH = c.speedYAW = 30 * SBGC_SPEED_SCALE;
c.anglePITCH = SBGC_DEGREE_TO_ANGLE(180);
c.angleYAW = SBGC_DEGREE_TO_ANGLE(360);
c.angleROLL = SBGC_DEGREE_TO_ANGLE(20);
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(4000);
c.anglePITCH = SBGC_DEGREE_TO_ANGLE(-180);
c.angleYAW = SBGC_DEGREE_TO_ANGLE(-360);
c.angleROLL = SBGC_DEGREE_TO_ANGLE(-20);
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(8000);
// .. and back
c.anglePITCH = 0;
c.angleYAW = 0;
c.angleROLL = 0;
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(4000);
}
/////////////////// Demo 2. Pitch gimbal down with constant speed 10 degree/sec
// by 50 degree (it takes 5 sec)
// (this is simplified version of speed control. To prevent jerks, you should
// add acceleration and de-acceleration phase)
void example2(SBGC_Parser sbgc_parser) {
SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 };
c.mode = SBGC_CONTROL_MODE_SPEED;
c.speedPITCH = 10 * SBGC_SPEED_SCALE;
c.speedROLL = 10 * SBGC_SPEED_SCALE;
c.speedYAW = 10 * SBGC_SPEED_SCALE;
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(5000);
// Stop
c.speedPITCH = 0;
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(1000);
// .. and back
c.speedPITCH = -100 * SBGC_SPEED_SCALE;
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(5000);
// Stop
c.speedPITCH = 0;
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(1000);
}
void comunication(SBGC_Parser sbgc_parser) {
SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 };
Gamepad mando(1);
Sleep(1000); //Take a pause to let gimbal controller to initializa
example0(sbgc_parser);
c.speedROLL = rollSpeed * SBGC_SPEED_SCALE;
c.speedPITCH = pichSpeed * SBGC_SPEED_SCALE;
c.speedYAW = yawSpeed * SBGC_SPEED_SCALE;
printf("--- Comienzo movimiento ---");
while (1) {
if (mando.GetButtonPressed(XButtons.A)) example0(sbgc_parser);
//Put mode and angles values
c.mode = SBGC_CONTROL_MODE_ANGLE;
//Actualizar el mando y obtener los valores para los 3 ejes.
mando.Update();
c.angleYAW = SBGC_DEGREE_TO_ANGLE(get_YAW(mando)); //eje x
c.anglePITCH = SBGC_DEGREE_TO_ANGLE(get_PICH(mando)); //eje y
//uint16_t roll = get_ROLL(mando);
float roll = get_ROLL(mando);
if (roll < -30) roll = -30;
else if (roll > 30) roll = 30;
c.angleROLL = SBGC_DEGREE_TO_ANGLE(roll); //eje z
//send new comtrol mode and values
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(20);
}
}
///////////// Demo3: Return control back to RC for 5 seconds
void example3(SBGC_Parser sbgc_parser) {
SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 };
c.mode = SBGC_CONTROL_MODE_NO;
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(5000);
}
void Gimbal::possInicial()
{
SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 };
c.mode = SBGC_CONTROL_MODE_ANGLE;
c.speedROLL = c.speedPITCH = c.speedYAW = 30 * SBGC_SPEED_SCALE;
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(1000);
}
void example0(SBGC_Parser sbgc_parser)
{
SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 };
c.mode = SBGC_CONTROL_MODE_ANGLE;
c.speedROLL = c.speedPITCH = c.speedYAW = 30 * SBGC_SPEED_SCALE;
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(3000);
}
bool Gimbal::movimientoVelocidad(int yaw, int pich)
{
//Modo de control, por velocidad, los datos iran en tanto por uno siendo 1 completamente pulsada la palanca y 0 nada.
c.mode = SBGC_CONTROL_MODE_SPEED;
//COMANDO ORIGINAL c.speedROLL= (50 * mando.LeftStick_X()) * SBGC_SPEED_SCALE; //eje x
//Obtener los valores para las direcciones
//eje x
if (posicion.yaw >= 90 && yaw >= 0)
c.speedYAW = 0 * SBGC_SPEED_SCALE;
else if (posicion.yaw >= 90 && yaw < 0)
c.speedYAW = (velYaw * yaw) * SBGC_SPEED_SCALE;
else if (posicion.yaw <= -90 && yaw <= 0)
c.speedYAW = 0 * SBGC_SPEED_SCALE;
else if (posicion.yaw <= -90 && yaw > 0)
c.speedYAW = velYaw * yaw * SBGC_SPEED_SCALE;
else c.speedYAW = yaw * velYaw * SBGC_SPEED_SCALE;
//eje y
if (posicion.pitch >= 45 && pich >= 0)
c.speedPITCH = 0 * SBGC_SPEED_SCALE;
else if (posicion.pitch >= 45 && pich < 0)
c.speedPITCH = (velPich * pich) * SBGC_SPEED_SCALE;
else if (posicion.pitch <= -45 && pich <= 0)
c.speedPITCH = 0 * SBGC_SPEED_SCALE;
else if (posicion.pitch <= -45 && pich > 0)
c.speedPITCH = (velPich * pich) * SBGC_SPEED_SCALE;
else c.speedPITCH = pich * velPich * SBGC_SPEED_SCALE;
//eje z
c.speedROLL = 0 * SBGC_SPEED_SCALE;
gimbalResponse = SBGC_cmd_control_send(c, sbgc_parser);
Sleep(SBGC_CMD_DELAY);
limpiarBuffer();
//actualizarPosicion();
//TODO
return gimbalResponse;
}
void comunicationSpeed(SBGC_Parser sbgc_parser) {
SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 };
Gamepad mando(1);
printf("\n %d", mando.Connected());
//Sleep(3000); //Take a pause to let gimbal controller to initializa
//example0(sbgc_parser);
do{
//Put mode and angles values
c.mode = SBGC_CONTROL_MODE_SPEED;
//Actualizar el mando y obtener los valores para los 3 ejes.
mando.Update();
printf("\n Lsx-%0.2f Lsy-%0.2f", mando.LeftStick_X(), mando.LeftStick_Y());
c.speedROLL= (50 * mando.LeftStick_X()) * SBGC_SPEED_SCALE; //eje x
c.speedPITCH = (50 * mando.LeftStick_Y()) * SBGC_SPEED_SCALE; //eje y
c.speedYAW = (50*mando.RightStick_X()) * SBGC_SPEED_SCALE; //eje z
//send new comtrol mode and values
SBGC_cmd_control_send(c, sbgc_parser);
Sleep(20);
} while(true);
}
void simplebgc_parse(void){
SBGC_cmd_control_t c = { 0, 0, 0, 0, 0, 0, 0 };
c.mode = SBGC_CONTROL_MODE_ANGLE;
c.speedROLL = c.speedPITCH = c.speedYAW = 30 * SBGC_SPEED_SCALE;
SBGC_cmd_control_send(c, sbgc_parser);
}
