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