Commit fae880b5 authored by bucyril's avatar bucyril
Browse files

reverted CrazyRadio, fixed PPSClient and fucked around with...

reverted CrazyRadio, fixed PPSClient and fucked around with SafeControllerService. Also modified .gitignore to ignore all pyc files.
parent 75c4aef4
pps_ws/build/
pps_ws/devel/
*.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpstack.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/radiodriver.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/exceptions.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/__init__.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/crtpdriver.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/usbdriver.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/udpdriver.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/debugdriver.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crtp/serialdriver.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/platformservice.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/param.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/commander.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/__init__.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/log.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/console.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toccache.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/mem.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/crazyflie/toc.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/__init__.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/crazyradio.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/drivers/cfusb.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/__init__.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/utils/callbacks.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/utils/__init__.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/boottypes.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/__init__.pyc
./pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/cloader.pyc
./pps_ws/src/d_fall_pps/crazyradio/leapsdk/__init__.pyc
pps_ws/src/d_fall_gui/GUI_Qt/build*
\ No newline at end of file
pps_ws/src/d_fall_gui/GUI_Qt/build*
......@@ -106,8 +106,8 @@ def angleCommandCallback(data):
def rateCommandCallback(data):
"""Callback for rate controller actions"""
#cmd1..4 must not be 0, as crazyflie onboard controller resets
#cf_client._send_to_commander(data.rollRate,data.pitchRate,data.yawRate,data.thrust, 1, 1, 1, 1, CONTROLLER_RATE)
cf_client._send_to_commander(0,0,0,0, 20000, 20000, 0, 0, 2)
cf_client._send_to_commander(data.rollRate,data.pitchRate,data.yawRate,data.thrust, 1, 1, 1, 1, CONTROLLER_RATE)
#cf_client._send_to_commander(0,0,0,0, 20000, 20000, 0, 0, 0)
#ACHTUNG: mode ist auf 2 (CONTROLLER_MOTOR), da die rateCommandCallback zum testen verwendet wurde!!!!!!!!!!!!!!!!!!!!!!!!
rospy.loginfo("rate controller callback : %s, %s, %s, %s", data.rollRate, data.pitchRate, data.yawRate, data.thrust)
......
......@@ -17,10 +17,10 @@
//TODO:
//CentralManager: extract data about room from vicon data
//CentralManager: assign area for each group and those coordinates to PPSClients
//CentralManager: assign localArea for each group and those coordinates to PPSClients
//ViconDataPublisher: extract data about room from vicon data in and send also to PPSClient
//PPSClient: Compare data received from CentralManager and ViconDataPublisher and determine in which area you are
//PPSClient: Choose correct controller accoring to current area
//PPSClient: Compare data received from CentralManager and ViconDataPublisher and determine in which localArea you are
//PPSClient: Choose correct controller accoring to current localArea
#include "ros/ros.h"
......@@ -60,18 +60,7 @@ d_fall_pps::RateCommand safeRateCommandPkg;
//uncommenting the next line causes FATAL Error at runtime: "You must call ros::init() before creating the first NodeHandle"
//ros::NodeHandle nodeHandle;
//move this section to a separate file that is included>>>>>>>>>
struct AreaBoundaries{
float areaBoundxmin;
float areaBoundxmax;
float areaBoundymin;
float areaBoundymax;
float areaBoundzmin;
float areaBoundzmax;
};
AreaBoundaries Area;
AreaBounds localArea;
//struct consistent with dusans controller
......@@ -156,13 +145,13 @@ bool safetyCheck(ViconData data){
//ROS_INFO_STREAM("ViconData: " << data.x << ", " << data.y << ", " << data.z);
//position check
if((data.x < Area.areaBoundxmin) or (data.x < Area.areaBoundxmax)){
if((data.x < localArea.xmin) or (data.x > localArea.xmax)){
return true;
}
if((data.y < Area.areaBoundymin) or (data.z < Area.areaBoundymax)){
if((data.y < localArea.ymin) or (data.y > localArea.ymax)){
return true;
}
if((data.z < Area.areaBoundzmin) or (data.z < Area.areaBoundzmax)){
if((data.z < localArea.zmin) or (data.z > localArea.zmax)){
return true;
}
......@@ -174,7 +163,7 @@ bool safetyCheck(ViconData data){
//is called upon every new arrival of data in main
void viconCallback(const d_fall_pps::ViconData& data){
ROS_INFO("in viconCallback");
//ROS_INFO("in viconCallback");
//ROS_INFO_STREAM(data);
//ROS_INFO("My teamname is:"); ROS_INFO_STREAM(team);
//ROS_INFO("My crazyflie is:"); ROS_INFO_STREAM(cflie);
......@@ -182,7 +171,7 @@ void viconCallback(const d_fall_pps::ViconData& data){
if(data.crazyflieName == cflie){
//forward data to safety check
bool autocontrolOn = safetyCheck(data);
ROS_INFO_STREAM("autocontrolOn: " << autocontrolOn);
//ROS_INFO_STREAM("autocontrolOn: " << autocontrolOn);
ppsClientToController(data, autocontrolOn);
}
else {
......@@ -294,14 +283,9 @@ int main(int argc, char* argv[]){
CentralManager ManagerSettings;
if(centralClient.call(ManagerSettings)){
Area.areaBoundxmin = ManagerSettings.response.context.localArea.xmin;
Area.areaBoundxmax = ManagerSettings.response.context.localArea.xmin;
Area.areaBoundymin = ManagerSettings.response.context.localArea.xmin;
Area.areaBoundymax = ManagerSettings.response.context.localArea.xmin;
Area.areaBoundzmin = ManagerSettings.response.context.localArea.xmin;
Area.areaBoundzmax = ManagerSettings.response.context.localArea.xmin;
localArea = ManagerSettings.response.context.localArea;
ROS_INFO("CentralManager responded");
ROS_INFO("AreaBoundaries Set");
ROS_INFO("localAreaBoundaries Set");
}
......
......@@ -73,7 +73,11 @@ bool calculateControlOutput(RateController::Request &request, RateController::Re
response.controlOutput.yawRate = -(k[18] * px + k[19] * py + k[20] * pz + k[21] * vx + k[22] * vy + k[23] * vz + k[24] * roll + k[25] * pitch + k[26] * yaw);
float thrustIntermediate = -(k[27] * px + k[28] * py + k[29] * pz + k[30] * vx + k[31] * vy + k[32] * vz + k[33] * roll + k[34] * pitch + k[35] * yaw);
//idea: linerazie plot and apply on sum of thrust instead of on each motor
response.controlOutput.thrust = 20000;
if(thrustIntermediate < 0) thrustIntermediate = 0;
response.controlOutput.thrust = (int) (428571 * thrustIntermediate);
ROS_INFO("??????????????????????????????????????????????????????????????????????????????????????");
ROS_INFO_STREAM(thrustIntermediate);
ROS_INFO_STREAM(response.controlOutput.thrust);
/*cmd1Thrust=(-m_a1+sqrt(m_a1*m_a1-4*m_a2*(m_a0-(thrust+m_ffCmd1Thrust))))/(2*m_a2);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment