JNIEXPORT void JNICALL
Java_ptolemy_apps_apes_CTask_CMethod(JNIEnv *env, jobject obj, jstring taskName)
{
const char *task = (*env)->GetStringUTFChars(env, taskName, 0);
if (strcmp(task, "DispatcherTask") == 0)
appDispatcher();
else if (strcmp(task, "DynamicsControllerTask") == 0)
dynaController();
else if (strcmp(task, "MotorControllerTask") == 0)
motorController();
else if (strcmp(task, "DispatcherIRS") == 0)
dispatcherIRS();
(*env)->ReleaseStringUTFChars(env, taskName, task);
}
// Update the controller
void GazeboRosTricycleDrive::UpdateChild()
{
if ( odom_source_ == ENCODER ) UpdateOdometryEncoder();
common::Time current_time = parent->GetWorld()->GetSimTime();
double seconds_since_last_update = ( current_time - last_actuator_update_ ).Double();
if ( seconds_since_last_update > update_period_ ) {
publishOdometry ( seconds_since_last_update );
if ( publishWheelTF_ ) publishWheelTF();
if ( publishWheelJointState_ ) publishWheelJointState();
double target_wheel_roation_speed = cmd_.speed / ( diameter_actuated_wheel_ / 2.0 );
double target_steering_angle = cmd_.angle;
motorController ( target_wheel_roation_speed, target_steering_angle, seconds_since_last_update );
//ROS_INFO("v = %f, w = %f !", target_wheel_roation_speed, target_steering_angle);
last_actuator_update_ += common::Time ( update_period_ );
}
}
int main(int argc, const char * argv[])
{
OptParse parser;
// parser.addOption("s", true);
parser.addOption("js", true);
if(!parser.parse(argc, argv))
{
std::cout << "Required Options were not specified" << std::endl;
return 1;
}
asio::io_service io;
PacketSender sender(io);
sender.connect(6142);
// for(int i = 0; i < 5; ++i)
// {
// sender.write("P");
// std::this_thread::sleep_for(std::chrono::milliseconds(1000));
// }
JoyStick joystick(parser.getOption("js"));
//AudioCapture audio("hw:1,0", "S16_LE");
//AudioCapture audio("hw:1,0", "");
//AudioCapture audio;
//ImageProcessor imageProcessor;
//SerialComm serial;
// Define the Axis to use
TimedAxisController panAxis(joystick, JoyStick::Axis::HAT_H);
panAxis.setInterval(50);
TimedAxisController tiltAxis(joystick, JoyStick::Axis::HAT_V);
tiltAxis.setInterval(100);
EventAxisController leftMotorAxis(joystick, JoyStick::Axis::LEFT_STICK_V);
EventAxisController rightMotorAxis(joystick, JoyStick::Axis::RIGHT_STICK_V);
CameraController cameraController(panAxis, tiltAxis, 90, 90);
cameraController.setPanSpeed(2.0f);
cameraController.setTiltSpeed(2.0f);
MotorController motorController(leftMotorAxis, rightMotorAxis);
motorController.setMaxSpeed(100.0f);
// main command dispatch loop
// read input from the joy stick and handle appropriate events
while(true)
{
joystick.pollEvents();
// Check for exit
if(joystick.isPressed(JoyStick::Button::BACK))
{
std::cout << "Exiting" << std::endl;
break;
}
// Attempt to sync with platform
if(joystick.isPressed(JoyStick::Button::START))
{
std::cout << "Requesting Sync with Platform..." << std::endl;
sender.write("Z");
// ...
}
// When the left trigger is pressed capture image from webcam
// if(joystick.isPressed(JoyStick::Button::LEFT_TRIGGER1))
// {
// std::cout << "Capturing Image..." << std::endl;
// imageProcessor.process();
// }
// Trigger Audio Capture
// if(joystick.isPressed(JoyStick::Button::RIGHT_TRIGGER1))
// {
// std::cout << "Capturing Audio..." << std::endl;
// audio.capture(44100, 3);
// }
cameraController.update(sender);
motorController.update(sender);
}
sender.close();
return 0;
}
JNIEXPORT void JNICALL
Java_ptolemy_apps_apes_demo_ARS_MotorControllerTask_CMethod(JNIEnv *env, jobject obj)
{
motorController();
return;
}
