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