float diff_angle(float angle1, float angle2) { float diff; diff = angle2-angle1; convert_angle(&diff); return diff; }
void controller(float dx, float dy, float dz, float angle_drone_cons, float angle_drone, float * pitch_cmd, float * roll_cmd, float * angular_speed_cmd, float * vertical_speed_cmd) { // Angle must be between -180 and +180° convert_angle(&angle_drone_cons); convert_angle(&angle_drone); // CHange of reference (from the room reference to the drone reference) room_to_drone(dx,dy,angle_drone,pitch_cmd,roll_cmd); // Apply gain *pitch_cmd *= -GAIN_PITCH; *roll_cmd *= GAIN_ROLL; *angular_speed_cmd = -GAIN_ANGULAR * diff_angle(angle_drone,angle_drone_cons); *vertical_speed_cmd = GAIN_VERTICAL * dz; //printf("pitch=%f\troll=%f\tangular_speed=%f\tvert_speed=%f\n",*pitch_cmd,*roll_cmd,*angular_speed_cmd,*vertical_speed_cmd); // Saturate the command if necessary if(*pitch_cmd>PITCH_CMD_MAX) { *pitch_cmd = PITCH_CMD_MAX; } else if(*pitch_cmd<-PITCH_CMD_MAX) { *pitch_cmd = -PITCH_CMD_MAX; } if(*roll_cmd>ROLL_CMD_MAX) { *roll_cmd = ROLL_CMD_MAX; } else if(*roll_cmd<-ROLL_CMD_MAX) { *roll_cmd = -ROLL_CMD_MAX; } if(*angular_speed_cmd>ANGULAR_CMD_MAX) { *angular_speed_cmd = ANGULAR_CMD_MAX; } else if(*angular_speed_cmd<-ANGULAR_CMD_MAX) { *angular_speed_cmd = -ANGULAR_CMD_MAX; } if(*vertical_speed_cmd>VERTICAL_CMD_MAX) { *vertical_speed_cmd = VERTICAL_CMD_MAX; } else if(*vertical_speed_cmd<-VERTICAL_CMD_MAX) { *vertical_speed_cmd = -VERTICAL_CMD_MAX; } }
void cmd_joints_Callback(const sensor_msgs::JointState js) { brazen_arm.cmd_status = 1; brazen_arm.servo_enable = 0xFFF; brazen_arm.servo1a_pos = convert_angle(js.position[0]); brazen_arm.servo2a_pos = convert_angle(js.position[1]); brazen_arm.servo3a_pos = convert_angle(js.position[2]); brazen_arm.servo4a_pos = 0xFF; //convert_angle(js.position[3]); brazen_arm.servo5a_pos = 0xFF; //convert_angle(js.position[4]); brazen_arm.servo6a_pos = 0xFF; //convert_angle(js.position[5]); brazen_arm.servo1b_pos = 0xFF; //convert_angle(js.position[6]); brazen_arm.servo2b_pos = 0xFF; //convert_angle(js.position[7]); brazen_arm.servo3b_pos = 0xFF; //convert_angle(js.position[8]); brazen_arm.servo4b_pos = 0xFF; //convert_angle(js.position[9]); brazen_arm.servo5b_pos = 0xFF; //convert_angle(js.position[10]); brazen_arm.servo6b_pos = 0xFF; //convert_angle(js.position[11]); }
void mission(float x_cons, float y_cons, float z_cons, float angle_cons, float * pitch_cmd, float * roll_cmd, float * angular_speed_cmd, float * vertical_speed_cmd) { pthread_mutex_lock(&mutex_mission); // Refresh X and Y if new data are available if(newCoordXY!=0) { x = loca_x; y = loca_y; newCoordXY = 0; } // Refresh Z if new data are available if(newCoordZ!=0) { z = navData_z; newCoordZ = 0; } // Refresh the heading if new data is available if(newAngle!=0) { angle = navData_angle; newAngle = 0; } // Calculate the error (input of the controller) float dx = x_cons - x; float dy = y_cons - y; float dz = z_cons - z; // Convert the angle in order that it is between -180 and +180° convert_angle(&angle); convert_angle(&angle_cons); //printf("dx=%f\tdy=%f\tdz=%f\tangle=%f\tangle_cons=%f\n",dx,dy,dz,angle,angle_cons); // Calculate the command (output of the controller / input of the drone) controller(dx, dy, dz, angle_cons, angle, pitch_cmd, roll_cmd, angular_speed_cmd, vertical_speed_cmd); pthread_mutex_unlock(&mutex_mission); }
void newNavData(float z_baro, float heading, float forward_backward_speed, float left_right_speed) { pthread_mutex_lock(&mutex_mission); // Save the new data navData_z = z_baro; navData_angle = heading-BIAIS; convert_angle(&navData_angle); navData_fb_speed = forward_backward_speed; navData_lr_speed = left_right_speed; // Indicate that new data are available newCoordZ = 1; newAngle = 1; newSpeed = 1; pthread_mutex_unlock(&mutex_mission); }
void cRocketTurret::think() { int iMyIndex = -1; for (int i = 0; i < MAX_STRUCTURES; i++) { if (structure[i] == this) { iMyIndex = i; break; } } // this should not happen, but just in case if (iMyIndex < 0) { return; } if (player[getOwner()].bEnoughPower() == false) { return; // do not fire a thing now } // turning & shooting if (iTargetID > -1) { if (unit[iTargetID].isValid()) { // first make sure we face okay! int iCellX = iCellGiveX(getCell()); int iCellY = iCellGiveY(getCell()); int iTargetX = iCellGiveX(unit[iTargetID].iCell); int iTargetY = iCellGiveY(unit[iTargetID].iCell); int d = fDegrees(iCellX, iCellY, iTargetX, iTargetY); int f = face_angle(d); // get the angle // set facing iShouldHeadFacing = f; if (iShouldHeadFacing == iHeadFacing || (unit[iTargetID].iType == ORNITHOPTER)) { TIMER_fire++; int iDistance = 9999; int iSlowDown = 250; if (getType() == RTURRET) iSlowDown = 450; if (unit[iTargetID].isValid()) { // calculate distance iDistance = ABS_length(iCellX, iCellY, iTargetX, iTargetY); if (iDistance > structures[getType()].sight) iTargetID = -1; } else iTargetID = -1; if (iTargetID < 0) return; if (TIMER_fire > iSlowDown) { int iTargetCell = unit[iTargetID].iCell; int iBullet = BULLET_TURRET; if (getType() == RTURRET && iDistance > 3) iBullet = ROCKET_RTURRET; else { int iShootX = (iDrawX() + 16) + (mapCamera->getX() * 32); int iShootY = (iDrawY() + 16) + (mapCamera->getY() * 32); int bmp_head = convert_angle(iHeadFacing); PARTICLE_CREATE(iShootX, iShootY, OBJECT_TANKSHOOT, -1, bmp_head); } int iBull = create_bullet(iBullet, getCell(), iTargetCell, -1, iMyIndex); if (unit[iTargetID].iType == ORNITHOPTER) { // it is a homing missile! bullet[iBull].iHoming = iTargetID; bullet[iBull].TIMER_homing = 200; } TIMER_fire = 0; } } else { TIMER_turn++; int iSlowDown = 200; if (TIMER_turn > iSlowDown) { TIMER_turn = 0; int d = 1; int toleft = (iHeadFacing + 8) - iShouldHeadFacing; if (toleft > 7) toleft -= 8; int toright = abs(toleft - 8); if (toright == toleft) d = -1 + (rnd(2)); if (toleft > toright) d = 1; if (toright > toleft) d = -1; iHeadFacing += d; if (iHeadFacing < 0) iHeadFacing = 7; if (iHeadFacing > 7) iHeadFacing = 0; } // turning } } else iTargetID = -1; } // think like base class cAbstractStructure::think(); }