void setRobotPhysLimit( int distance, char direction, int height, int roller, int tray, int platform, int timer )
{
ClearTimer( T1 );
resetEncoders();
//artificiallyresetGyro();
while(time1[T1] < timer && !SensorValue[TowerLimitL] && !SensorValue[TowerLimitR] )
{
if( direction == 'S' )
setDrive( distance );
else if( direction == 'T' )
spinDrive( distance );
else
wheelDrive( distance, direction );
//setArm( height );
if( height == 0 )
moveArm( -10, -10 );
else
{
if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 1 )
moveArm( 7, 7 );
else if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 0 )
moveArm( 7, 20 );
else if( SensorValue[ButtonBlockL] == 0 && SensorValue[ButtonBlockR] == 1 )
moveArm( 20, 7 );
else
setArm( height );
}
moveRollers( roller, roller );
movePiston( tray, platform );
}
stopMotors();
}
//MAIN//
task main()
{
initializeRobot();
waitForStart();
servo[intakeServo] = 80;
//SAFE BEGIN//
//---Lift arm
moveArm(80);
wait10Msec(60);
moveArm(0);
//---Move forward
moveStraight(6.0,50);
//---Turn Right
gyroCenterPivot(-30, 100);
stopDriveTrain();
//---Score
servo[intakeServo] = 150;
wait1Msec(700);
//---Move forward to get on ramp
moveStraight(12.0, -100);
stopDriveTrain();
gyroCenterPivot(45,100);
stopDriveTrain();
moveStraight(60.0,100);
stopDriveTrain();
//---Close box
servo[intakeServo] = 80;
wait1Msec(5000);
}
task main()
{
waitForStart();
moveStraight(50,170);
move(-50,50,430);
moveArm(-75,700);
moveHand(160);
moveArm(-75,800);
moveHand(140);
moveStraight(30,1450);
moveArm(-75,700);
wait1Msec(1000);
moveStraight(-30,400);
move(-50,50,430);
moveStraight(-50,800);
/* moveStraight(50,1630);
move(50,-50,390);
moveArm(-75,700);
moveHand(160);
moveArm(-75,900);
moveHand(140);
moveStraight(30,800);
moveArm(-75,700);
wait1Msec(1000);
moveStraight(-30,500);
move(-50,50,500);
moveStraight(-30,1650);
moveArm(75,2000);*/
while (true)
{}
}
bool AngularMotor::setInitialStandUpBack(){
effect.str("");
addvalue();
moveArm(right_side, addraj1, addraj2, addraj3, addraj4);
moveLeg(right_side, addrlj1, addrlj2, addrlj3, addrlj4, addrlj5, addrlj6);
moveArm(left_side, addlaj1, addlaj2, addlaj3, addlaj4);
moveLeg(left_side, addllj1, addllj2, addllj3, addllj4, addllj5, addllj6);
return true;
}
bool AngularMotor::allJointsOff(){
effect.str("");
moveArm(right_side, 0, 0, 0, 0);
moveArm(left_side, 0, 0, 0, 0);
moveLeg(right_side, 0, 0, 0, 0, 0, 0);
moveLeg(left_side, 0, 0, 0, 0, 0, 0);
moveHead(0, 0);
return true;
}
void manualArmDownComplex()
{
if( SensorValue[PotentL] > SensorValue[PotentR] )
moveArm( (int)( -40.0 ), (int)( -40.0 * 0.9 ) );
else if( SensorValue[PotentL] < SensorValue[PotentR] )
moveArm( (int)( -40.0 * 0.9 ), (int)( -40.0 ) );
else
moveArm( (int)( -40.0 ), (int)( -40.0 ) );
}
void apprentissageAleatoire(fonction& g, bool first_pass, bool record){
BaseEtats baseEtats;
Thetas thetarandom;
Thetas dtheta;
Thetas dthetamin;
Rayon rmin;
Rayon r;
std::list<gaml::libsvm::Predictor<Entree,double>> predictors;
std::array<std::string,4> filenames = {{std::string("theta1.pred"),"theta2.pred","theta3.pred","theta4.pred"}};
for (int j=0;j< NB_TESTS_PAR_PHASE;j++){
std::cout<<"Nouvelle boucle mise a jour de f"<<std::endl;
std::cout<<"____________________"<< std::endl;
std::cout<<"Nouvel essai random n°"<<j<<std::endl;
r = {0,0,0};
while(r[0] == 0 && r[1] == 0 && r[2] == 0){
thetarandom = creationThetaRandom();
moveArm(thetarandom);
r = vecteur_kinnect_objet();
}
if(first_pass){
dtheta = {.0,.0,.0,.0};
}else{
auto res = g({r[0],r[1],r[2],thetarandom[0],thetarandom[1],thetarandom[2],thetarandom[3]});
dtheta = {
res.theta1,
res.theta2,
res.theta3,
res.theta4
};
}
moveArm(thetarandom+dtheta);
rmin = vecteur_kinnect_objet();
std::cout<<"Thetas :"<<thetarandom<<" | Rayon :"<<r<< " | Distance :"<<norme(r)<<" | dthetas :"<<dtheta<<std::endl;
dthetamin = oscillations(r,thetarandom,rmin,dtheta);
Etat etat (r,thetarandom,dthetamin);
baseEtats.push_back(etat);
std::cout<<"Amelioration dthetas : "<<dthetamin<<std::endl;
}
std::cout<<"mise a jour de f"<<std::endl;
g = calcul_f(baseEtats,record);
}
///////////TASKS///////////////
task Arm()
{
while(needArm)
{
moveArm(100);
wait1Msec(750);
moveArm(0);
needArm = false;
}
}
void manualArmUpSimple()
{
if( SensorValue[ButtonBlockL] == 0 && SensorValue[ButtonBlockR] == 0 )
moveArm( 127, 127 );
else if( SensorValue[ButtonBlockL] == 1 && SensorValue[ButtonBlockR] == 0 )
moveArm( 3, 20 );
else if( SensorValue[ButtonBlockL] == 0 && SensorValue[ButtonBlockR] == 1 )
moveArm( 20, 3 );
else
moveArm( 3, 3 );
}
//------------Task to move the arm concurrently
task Arm()
{
while(true)
{
if(needArm)
{
moveArm(80);
wait1Msec(500);
moveArm(0);
return;
}
}
}
void teleop()
{
driveMotors(vexRT[Ch3], vexRT[Ch2]);
int armSpeed = 0;
if(vexRT[Btn5U])
{
armSpeed = 127;
}
else if(vexRT[Btn5D])
{
armSpeed = -127;
}
moveArm(armSpeed);
if(vexRT[Btn6U])
{
armSpeed = 127;
}
else if(vexRT[Btn6D])
{
armSpeed = -127;
}
moveArmTop(armSpeed);
}
task main()
{
initReadMode("auton.txt", 1024);
int nSteps = getSteps();
for(int i = 0; i < nSteps; i++) {
int* stps = getNextStep();
for(int m = 0; m < 5; m++) {
nMotorEncoder[mtrs[m]] = 0;
nMotorEncoderTarget[mtrs[m]] = stps[m];
motor[mtrs[m]] = 80 * sgn(stps[m]);
}
for(int s = 0; s < 2; s++) {
servo[servos[s]] = stps[s+6];
}
motor[ScissorLeft] = 80 * sgn(stps[5]);
while(nMotorRunState[ScissorRight] != IDLE) {};
motor[ScissorLeft] = 0;
}
moveArm();
}
void setArm( int point )
{
float maxPower = 127.0; // Limit Constants
float integralLimit = 20.0;
float P = .31415; // PID Constants
float I = 0.0003;
float D = 0.25;
float error = point - getPotentLValue(); // PID Calculations
integral_A += error;
float derivative = error - prev_A;
if( abs( error ) > integralLimit || error == 0 ) // Integral Limit Filter
integral_A = 0;
prev_A = error; // Derivative Reset
float power = P * error + I * integral_A + D * derivative; // Power Calculation
if( abs( power ) > maxPower ) // Power Limit Filter
power = power < 0 ? .9 * -maxPower : maxPower;
moveArm( power, power );
}
Thetas oscillations(Rayon r,Thetas& theta, Rayon rmin,Thetas& dthetamin){
Rayon rprim;
Thetas dthetaprim;
Rayon newrmin = rmin;
Thetas newdthetamin = dthetamin;
Thetas epsilon;
for(int i =0;i< NB_OSCILLATIONS;i++){
std::cout << "---------------------" << std::endl;
std::cout << "Amelioration n°" << i << std::endl;
epsilon = creationEpsilonAleatoire();
dthetaprim = dthetamin+epsilon;
moveArm(dthetaprim+theta);
rprim = vecteur_kinnect_objet();
std::cout<< "Distance : " << norme(rprim) << std::endl;
if(norme(rprim)<norme(newrmin)){
newrmin = rprim;
newdthetamin = dthetaprim;
}
}
return newdthetamin;
}
task main()
{
waitForStart();
moveStraight(-50,500);
move(50,-50,375);
moveArm(75,700);
moveHand(160);
moveArm(75,600);
moveHand(110);
moveStraight(-30,1400);
moveArm(75,700);
while (true)
{}
}
bool SpherePickingRobotNavigator::moveArmToSide()
{
updateChangesToPlanningScene();
_JointConfigurations.fetchParameters(JOINT_CONFIGURATIONS_NAMESPACE);
return moveArm(arm_group_name_,_JointConfigurations.SideAngles);
}
//--------------------------------------------------------------
void ofApp::update(){
paths.update();
moveArm();
robot.update();
updateActiveCamera();
}
//MAIN//
task main()
{
initializeRobot();
waitForStart();
moveArm(ARM_UP); //lift arm
wait10Msec(80);
moveStraight(90.0, 10.0, 50.0); //move forward to crate
dropTheBlock();
while(true){};
}
//--------------------------------------------------------------
void ofApp::update(){
if (paths.size() > 0){
paths.update();
}
updateMocap();
moveArm();
robot.update();
updateActiveCamera();
}
void checkSideAndSwoop(int isRight)
{
//Drive forwards
driveMotors(70,70);
wait1Msec(1000);
driveMotors(0,0);
wait1Msec(5);
//Knock off starting cube
moveArm(127);
wait1Msec(1000);
moveArm(0);
wait1Msec(5);
//Continue straight
driveMotors(70,70);
wait1Msec(800);
driveMotors(0,0);
wait1Msec(5);
driveMotors(120,-120);
wait1Msec(2600);
driveMotors(0,0);
wait1Msec(5);
driveMotors(70,70);
wait1Msec(1800);
driveMotors(0,0);
wait1Msec(5);
driveMotors(-70,-70);
wait1Msec(1000);
driveMotors(0,0);
wait1Msec(5);
}
void UC_arm(int safe) {
//if (safe) {
if (joystickGetDigital(1, 5, JOY_UP)
&& joystickGetDigital(1, 5, JOY_DOWN)) {
moveArm(0);
} else if (joystickGetDigital(1, 5, JOY_UP)) { // bring the arm up
moveArm(127);
} else if (joystickGetDigital(1, 5, JOY_DOWN)) { // bring the arm down
moveArm(-127);
} else { // don't do anything to the arm
moveArm(0);
}
/*} else {
if (vexRT[Btn5U] == 1 && vexRT[Btn5D] == 1) {
moveArm(0);
} else if (vexRT[Btn5U] == 1) { // bring the arm up
moveArm(127);
} else if (vexRT[Btn5D] == 1) { // bring the arm down
moveArm(-127);
} else { // don't do anything to the arm
moveArm(0);
}
}*/
}
/**
* Release gripper and deliver the object
*/
uint8_t Controller::release()
{
mBusy = true;
//open gripper
setGripper(true);
usleep(1000000);
//Close gripper and move arm back
std_msgs::Bool bool_msg;
bool_msg.data = true;
mGripperClosePublisher.publish(bool_msg);
moveArm(MIN_ARM_X_VALUE, MIN_ARM_Z_VALUE);
//Reset arousal and listen to feedback
mArousal = NEUTRAL_AROUSAL;
double currentTime = ros::Time::now().toSec();
int sleep_rate;
mNodeHandle.param<int>("node_sleep_rate", sleep_rate, 50);
ros::Rate sleep(sleep_rate);
//wait for feedback
while(ros::ok() && ros::Time::now().toSec() - currentTime < 30 && mArousal == NEUTRAL_AROUSAL)
{
sleep.sleep();
ros::spinOnce();
}
if(mArousal > NEUTRAL_AROUSAL)
{
mBusy = false;
return nero_msgs::Emotion::HAPPY;
}
else if(mArousal < NEUTRAL_AROUSAL)
{
mBusy = false;
return nero_msgs::Emotion::SAD;
}
else
{
setFocusFace(false);
mBusy = false;
return nero_msgs::Emotion::NEUTRAL;
}
}
task main() {
initializeRobot();
while (true) {
// Get current joystick buttons and analog movements
getJoystickSettings(joystick);
// Move robot
moveArcade();
// Move arm
moveArm();
// Move gripper
moveGripper();
}
}
// controls arm movement of robot
void UC_arm(bool safe) {
if(safe) {
if (vexRT[Btn5U] == 1 && vexRT[Btn5D] == 1) {
moveArm(0);
} else if (vexRT[Btn5U] == 1) { // bring the arm up
moveArm(127);
} else if (vexRT[Btn5D] == 1) { // bring the arm down
moveArm(-127);
} else { // don't do anything to the arm
moveArm(0);
}
} else {
if (vexRT[Btn5U] == 1 && vexRT[Btn5D] == 1) {
moveArm(0);
} else if (vexRT[Btn5U] == 1) { // bring the arm up
moveArm(127);
} else if (vexRT[Btn5D] == 1) { // bring the arm down
moveArm(-127);
} else { // don't do anything to the arm
moveArm(0);
}
}
}
//--------------------------------------------------------------
void ofApp::update(){
if(nodeTrack->lockNodeToTrack && !parameters.bCopy){
gizmo.setNode(tcpNode);
}else{
tcpNode.setTransformMatrix( gizmo.getMatrix() );
}
moveArm();
robot.update();
bool bInTimelineRect = timeline.getDrawRect().inside( ofGetMouseX(), ofGetMouseY() );
if (viewportReal.inside(ofGetMouseX(), ofGetMouseY()) && !bInTimelineRect )
{
activeCam = 0;
if(!cams[0]->getMouseInputEnabled()){
cams[0]->enableMouseInput();
}
if(cams[1]->getMouseInputEnabled()){
cams[1]->disableMouseInput();
}
}
if(viewportSim.inside(ofGetMouseX(), ofGetMouseY()) && !bInTimelineRect )
{
activeCam = 1;
if(!cams[1]->getMouseInputEnabled()){
cams[1]->enableMouseInput();
}
if(cams[0]->getMouseInputEnabled()){
cams[0]->disableMouseInput();
}
if(gizmo.isInteracting() && cams[1]->getMouseInputEnabled()){
cams[1]->disableMouseInput();
}
}
}
int HandymanTeleopKey::run(int argc, char **argv)
{
char c;
/////////////////////////////////////////////
// get the console in raw mode
int kfd = 0;
struct termios cooked;
struct termios raw;
tcgetattr(kfd, &cooked);
memcpy(&raw, &cooked, sizeof(struct termios));
raw.c_lflag &=~ (ICANON | ECHO);
raw.c_cc[VEOL] = 1;
raw.c_cc[VEOF] = 2;
tcsetattr(kfd, TCSANOW, &raw);
/////////////////////////////////////////////
showHelp();
ros::init(argc, argv, "handyman_teleop_key");
ros::NodeHandle node_handle;
// Override the default ros sigint handler.
// This must be set after the first NodeHandle is created.
signal(SIGINT, rosSigintHandler);
ros::Rate loop_rate(40);
std::string sub_msg_to_robot_topic_name;
std::string pub_msg_to_moderator_topic_name;
std::string sub_joint_state_topic_name;
std::string pub_base_twist_topic_name;
std::string pub_arm_trajectory_topic_name;
std::string pub_gripper_trajectory_topic_name;
node_handle.param<std::string>("sub_msg_to_robot_topic_name", sub_msg_to_robot_topic_name, "/handyman/message/to_robot");
node_handle.param<std::string>("pub_msg_to_moderator_topic_name", pub_msg_to_moderator_topic_name, "/handyman/message/to_moderator");
node_handle.param<std::string>("sub_joint_state_topic_name", sub_joint_state_topic_name, "/hsrb/joint_states");
node_handle.param<std::string>("pub_base_twist_topic_name", pub_base_twist_topic_name, "/hsrb/opt_command_velocity");
node_handle.param<std::string>("pub_arm_trajectory_topic_name", pub_arm_trajectory_topic_name, "/hsrb/arm_trajectory_controller/command");
node_handle.param<std::string>("pub_gripper_trajectory_topic_name", pub_gripper_trajectory_topic_name, "/hsrb/gripper_trajectory_controller/command");
ros::Subscriber sub_msg = node_handle.subscribe<handyman::HandymanMsg>(sub_msg_to_robot_topic_name, 100, &HandymanTeleopKey::messageCallback, this);
ros::Publisher pub_msg = node_handle.advertise<handyman::HandymanMsg>(pub_msg_to_moderator_topic_name, 10);
ros::Subscriber sub_joint_state = node_handle.subscribe<sensor_msgs::JointState>(sub_joint_state_topic_name, 10, &HandymanTeleopKey::jointStateCallback, this);
ros::Publisher pub_base_twist = node_handle.advertise<geometry_msgs::Twist> (pub_base_twist_topic_name, 10);
ros::Publisher pub_arm_trajectory = node_handle.advertise<trajectory_msgs::JointTrajectory>(pub_arm_trajectory_topic_name, 10);
ros::Publisher pub_gripper_trajectory = node_handle.advertise<trajectory_msgs::JointTrajectory>(pub_gripper_trajectory_topic_name, 10);
const float linear_coef = 0.2f;
const float linear_oblique_coef = 0.141f;
const float angular_coef = 0.5f;
float move_speed = 1.0f;
bool is_hand_open = false;
std::string arm_lift_joint_name = "arm_lift_joint";
std::string arm_flex_joint_name = "arm_flex_joint";
std::string wrist_flex_joint_name = "wrist_flex_joint";
while (ros::ok())
{
if(canReceive(kfd))
{
// get the next event from the keyboard
if(read(kfd, &c, 1) < 0)
{
perror("read():");
exit(EXIT_FAILURE);
}
switch(c)
{
case KEYCODE_0:
{
sendMessage(pub_msg, MSG_I_AM_READY);
break;
}
case KEYCODE_1:
{
sendMessage(pub_msg, MSG_ROOM_REACHED);
break;
}
case KEYCODE_2:
{
sendMessage(pub_msg, MSG_OBJECT_GRASPED);
break;
}
case KEYCODE_3:
{
sendMessage(pub_msg, MSG_TASK_FINISHED);
break;
}
case KEYCODE_6:
{
sendMessage(pub_msg, MSG_DOES_NOT_EXIST);
break;
}
case KEYCODE_9:
{
sendMessage(pub_msg, MSG_GIVE_UP);
break;
}
case KEYCODE_UP:
{
ROS_DEBUG("Go Forward");
moveBase(pub_base_twist, +linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_DOWN:
{
ROS_DEBUG("Go Backward");
moveBase(pub_base_twist, -linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_RIGHT:
{
ROS_DEBUG("Go Right");
moveBase(pub_base_twist, 0.0, 0.0, -angular_coef*move_speed);
break;
}
case KEYCODE_LEFT:
{
ROS_DEBUG("Go Left");
moveBase(pub_base_twist, 0.0, 0.0, +angular_coef*move_speed);
break;
}
case KEYCODE_S:
{
ROS_DEBUG("Stop");
moveBase(pub_base_twist, 0.0, 0.0, 0.0);
break;
}
case KEYCODE_U:
{
ROS_DEBUG("Move Left Forward");
moveBase(pub_base_twist, +linear_oblique_coef*move_speed, +linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_I:
{
ROS_DEBUG("Move Forward");
moveBase(pub_base_twist, +linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_O:
{
ROS_DEBUG("Move Right Forward");
moveBase(pub_base_twist, +linear_oblique_coef*move_speed, -linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_J:
{
ROS_DEBUG("Move Left");
moveBase(pub_base_twist, 0.0, +linear_coef*move_speed, 0.0);
break;
}
case KEYCODE_K:
{
ROS_DEBUG("Stop");
moveBase(pub_base_twist, 0.0, 0.0, 0.0);
break;
}
case KEYCODE_L:
{
ROS_DEBUG("Move Right");
moveBase(pub_base_twist, 0.0, -linear_coef*move_speed, 0.0);
break;
}
case KEYCODE_M:
{
ROS_DEBUG("Move Left Backward");
moveBase(pub_base_twist, -linear_oblique_coef*move_speed, +linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_COMMA:
{
ROS_DEBUG("Move Backward");
moveBase(pub_base_twist, -linear_coef*move_speed, 0.0, 0.0);
break;
}
case KEYCODE_PERIOD:
{
ROS_DEBUG("Move Right Backward");
moveBase(pub_base_twist, -linear_oblique_coef*move_speed, -linear_oblique_coef*move_speed, 0.0);
break;
}
case KEYCODE_Q:
{
ROS_DEBUG("Move Speed Up");
move_speed *= 2;
if(move_speed > 2 ){ move_speed=2; }
break;
}
case KEYCODE_Z:
{
ROS_DEBUG("Move Speed Down");
move_speed /= 2;
if(move_speed < 0.125){ move_speed=0.125; }
break;
}
case KEYCODE_Y:
{
ROS_DEBUG("Move Arm Up");
moveArm(pub_arm_trajectory, arm_lift_joint_name, 0.69, std::max<int>((int)(std::abs(0.69 - arm_lift_joint_pos1_) / 0.05), 1));
break;
}
case KEYCODE_H:
{
ROS_DEBUG("Stop Arm");
moveArm(pub_arm_trajectory, arm_lift_joint_name, 2.0*arm_lift_joint_pos1_-arm_lift_joint_pos2_, 0.5);
break;
}
case KEYCODE_N:
{
ROS_DEBUG("Move Arm Down");
moveArm(pub_arm_trajectory, arm_lift_joint_name, 0.0, std::max<int>((int)(std::abs(0.0 - arm_lift_joint_pos1_) / 0.05), 1));
break;
}
case KEYCODE_A:
{
ROS_DEBUG("Rotate Arm - Vertical");
moveArm(pub_arm_trajectory, arm_flex_joint_name, 0.0, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, -1.57, 1);
break;
}
case KEYCODE_B:
{
ROS_DEBUG("Rotate Arm - Upward");
moveArm(pub_arm_trajectory, arm_flex_joint_name, -0.785, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, -0.785, 1);
break;
}
case KEYCODE_C:
{
ROS_DEBUG("Rotate Arm - Horizontal");
moveArm(pub_arm_trajectory, arm_flex_joint_name, -1.57, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, 0.0, 1);
break;
}
case KEYCODE_D:
{
ROS_DEBUG("Rotate Arm - Downward");
moveArm(pub_arm_trajectory, arm_flex_joint_name, -2.2, 1);
moveArm(pub_arm_trajectory, wrist_flex_joint_name, 0.35, 1);
break;
}
case KEYCODE_G:
{
moveHand(pub_gripper_trajectory, is_hand_open);
is_hand_open = !is_hand_open;
break;
}
}
}
ros::spinOnce();
loop_rate.sleep();
}
/////////////////////////////////////////////
// cooked mode
tcsetattr(kfd, TCSANOW, &cooked);
/////////////////////////////////////////////
return EXIT_SUCCESS;
}
task main()
{
waitForStart(); // Wait for the tele-op period to begin
nMotorEncoder(motorLift1) = 0; // Reset the motor encoders for the arm
servoTarget(clawL) = 225; // Initialize the claw to be open
servoTarget(clawR) = 60;
PlaySoundFile("Leroy.rso"); // Shout "LEEROY JENKINS!", just for fun
wait1Msec(10); // Wait one tenth of a second
while (true)
{
getJoystickSettings(joystick); // Read the value of the joysticks
nxtDisplayTextLine(6, "Encoder: %d", nMotorEncoder[motorLift1]); // Display the value of the arm encoder
// The following code makes the directional pad on controller 1 give precise digital control of the drivetrain:
if (joystick.joy1_TopHat == 0) // If "up" on the directional pad is pressed:
{
go_forward(25); // Go forward at 25% speed
}
else if (joystick.joy1_TopHat == 1) // Else if "top right" on the directional pad is pressed:
{
strafe_forward_right(25); // Strafe diagonally forward and to the right at 25% speed
}
else if (joystick.joy1_TopHat == 2) // Else if "right" on the directional pad is pressed:
{
strafe_right(25); // Strafe to the right at 25% speed
}
else if (joystick.joy1_TopHat == 3) // Else if "bottom right" on the directional pad is pressed:
{
strafe_backward_right(25); // Strafe diagonally backward and to the right 25% speed
}
else if (joystick.joy1_TopHat == 4) // Else if "down" on the directional pad is pressed:
{
go_backward(25); // Go backward 25% speed
}
else if (joystick.joy1_TopHat == 5) // Else if "bottom left" on the directional pad is pressed:
{
strafe_backward_left(25); // Strafe diagonally backward and to the left 25% speed
}
else if (joystick.joy1_TopHat == 6) // Else if "left" on the directional pad is pressed:
{
strafe_left(25); // Strafe left 25% speed
}
else if (joystick.joy1_TopHat == 7) // Else if "top left" on the directional pad is pressed:
{
strafe_forward_left(25); // Strafe diagonally forward and to the left 25% speed
}
else if (joy1Btn(5)) // Else if button 6 is pressed:
{
rotate_clockwise(25); // Rotate the robot clockwise 25% speed
}
else if (joy1Btn(6)) // Else if button 5 is pressed:
{
rotate_counter_clockwise(25); // Rotate the robot counter-clockwise 25% speed
}
// The following code makes the joysticks on controller 1 give analogue control of the drivetrain:
else
{
motor[motorFL] = scale_motor(joystick.joy1_y1 + joystick.joy1_x1 + joystick.joy1_x2); // Scale the motors to the average
motor[motorFR] = scale_motor(joystick.joy1_y1 + -joystick.joy1_x1 - joystick.joy1_x2); //of the left joystick Y value and
motor[motorBR] = scale_motor(joystick.joy1_y1 + joystick.joy1_x1 - joystick.joy1_x2); //the right joystick X value
motor[motorBL] = scale_motor(joystick.joy1_y1 + -joystick.joy1_x1 + joystick.joy1_x2);
}
// The following code assigns the shoulder buttons on controller 2 to open and close the claw:
if (joy2Btn(5))
{
servoTarget(clawL) = 55; // If button 5 is pressed on controller 2, close the claw
servoTarget(clawR) = 170;
}
else if (joy2Btn(6))
{
servoTarget(clawL) = 115; // If button 6 is pressed on controller 2, open the claw
servoTarget(clawR) = 100;
}
else if (joy2Btn(8))
{
servoTarget(clawL) = ServoValue(clawL) + 1; // While button 7 is held, slowly close the claw
servoTarget(clawR) = ServoValue(clawR) - 1;
}
else if (joy2Btn(7))
{
servoTarget(clawL) = ServoValue(clawL) - 1; // While button 8 is held, slowly open the claw
servoTarget(clawR) = ServoValue(clawR) + 1;
}
else
{
servoTarget(clawL) = ServoValue(clawL); // Otherwise, do not move the claw
servoTarget(clawR) = ServoValue(clawR);
}
// The following code gives analogue control of the arm with the joystick, and assigns buttons 1-4 to move the arm to their respective rows on the scoring rack
if (joy2Btn(1)) // If button 1 is pressed, move the arm to the lowest row on the scoring rack
{
moveArm(level1Value);
}
else if (joy2Btn(2)) // If button 2 is pressed, move the arm to the middle row on the scoring rack
{
moveArm(level2Value);
}
else if (joy2Btn(3)) // If button 3 is pressed, move the arm to the top row on the scoring rack
{
moveArm(level3Value);
}
else if (joy2Btn(4)) // If button 4 is pressed, lower the arm all the way
{
moveArm(0);
}
else
{
motor[motorLift1] = (joystick.joy2_y1 / 12); // Otherwise, control the arm with joystick 1 on controller 2
motor[motorLift1] = (joystick.joy2_y1 / 24); // Or control the arm at half speed with joystick 2 on controller 2
// Note: these values are divided by 12 and 24 to bring the arm speed down to a reasonable level
}
}
}
int main(int argc, char **argv)
{
//So, for some odd reason involving the initialization of static non-copyable data, i can not get GUI to hold it's own RenderWindow
//So i'm going to bite the bullet and just do it here in this class... :P
gui.init();
//Create my BodyState tracker
stateTracker=BodyState();
//Set up some ROS stuff
ros::init(argc, argv, "mirror");
ros::NodeHandle n;
//First my subscribers
ros::Subscriber myoIMU_listener = n.subscribe("/myo/imu",1000, myoIMUCallBack);
ros::Subscriber myoGesture_listener = n.subscribe("/myo/gesture",1000, myoGestureCallBack);
ros::Subscriber myoOnboardGesture_listener = n.subscribe("/myo/onboardGesture",1000, myoOnboardGestureCallBack);
ros::Subscriber left_eye_watcher = n.subscribe("/multisense/left/image_rect/compressed",1000, leftEyeCallBack);
ros::Subscriber right_eye_watcher = n.subscribe("/multisense/right/image_rect/compressed",1000, rightEyeCallBack);
ros::Subscriber joint_listener = n.subscribe("/ihmc_ros/valkyrie/output/joint_states", 1000, jointStateCallBack);
ros::Subscriber transform_listener = n.subscribe("/tf", 1000, TFCallBack);
ros::Subscriber people_listener = n.subscribe("/people", 1000, peopleCallBack);
//Now my publishers
ros::Publisher arm_controller = n.advertise<ihmc_msgs::ArmTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/arm_trajectory", 1000);
ros::Publisher neck_controller = n.advertise<ihmc_msgs::NeckTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/neck_trajectory", 1000);
ros::Publisher pelvis_height_controller = n.advertise<ihmc_msgs::PelvisHeightTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/pelvis_height_trajectory", 1000);
ros::Publisher chest_controller = n.advertise<ihmc_msgs::ChestTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/chest_trajectory", 1000);
ros::Publisher go_home_pub = n.advertise<ihmc_msgs::GoHomeRosMessage>("/ihmc_ros/valkyrie/control/go_home",1000);
//Some SFML stufsf::RenderWindow windowf
sf::RenderWindow window;
if(gui.isOcculusOn){
sf::RenderWindow window(sf::VideoMode(2248, 544), "SFML works!");
sf::CircleShape shape(100.f);
shape.setFillColor(sf::Color::Green);
}
//Now the primary loop
ros::Rate loop_rate(6);
ihmc_msgs::GoHomeRosMessage goHomeMsg;
goHomeMsg.trajectory_time=3;
goHomeMsg.unique_id=14;
long int end, start;
while (ros::ok()){
struct timeval tp;
gettimeofday(&tp, NULL);
long int start = tp.tv_sec * 1000 + tp.tv_usec / 1000;
//std::cout<<tracking<<td::endl;
//Some more SFML stuff
if(GUIwindow.isOpen()){
//TODO:add in some human data gathering and print out human position and robot position data
//Also, add a table for Myo stuff
pollEventsGUI();
updateGUI();
}
//std::cout<<lastLeftEye.height<<" "<<lastLeftEye.width<<" "<<lastLeftEye.encoding<<" "<<lastLeftEye.step<<" "<<lastLeftEye.data.size()<<" "<<1024*544*3<<std::endl;
if(gui.isOcculusOn && window.isOpen() && lastLeftEye.data.size()!=0 && lastRightEye.data.size()!=0){
sf::Event event;
while (window.pollEvent(event))
{
if (event.type == sf::Event::Closed)
window.close();
}
sf::Uint8* pixels = new sf::Uint8[2248 * 544 * 4];
sf::Image image,imageL,imageR;
sf::Uint8* Ldata=new sf::Uint8[(size_t)lastLeftEye.data.size()];
sf::Uint8* Rdata=new sf::Uint8[(size_t)lastRightEye.data.size()];
for(int i=0;i<(size_t)lastLeftEye.data.size();i++){
Ldata[i]=lastLeftEye.data[i];
}
for(int i=0;i<(size_t)lastRightEye.data.size();i++){
Rdata[i]=lastRightEye.data[i];
}
imageL.loadFromMemory(Ldata,(size_t)lastLeftEye.data.size());
imageR.loadFromMemory(Rdata,(size_t)lastRightEye.data.size());
delete Ldata;
delete Rdata;
if(imageL.getSize().x!=imageR.getSize().x || imageL.getSize().y!=imageR.getSize().y)
std::cout<<"imageL:("<<imageL.getSize().x<<","<<imageL.getSize().y<<") imageR:("<<imageR.getSize().x<<","<<imageR.getSize().y<<")"<<std::endl;
sf::Texture texture;
int max_x=imageL.getSize().x;
int max_y=imageL.getSize().y;
for(int y = 0; y <max_y; y++)
{
for(int x = 0; x < max_x; x++)
{
pixels[(y*(2*max_x+200)+x)*4] = imageL.getPixelsPtr()[((max_y-y)*max_x+x)*4]; // R?
pixels[(y*(2*max_x+200)+x)*4 + 1] = imageL.getPixelsPtr()[((max_y-y)*max_x+x)*4+1]; // G?
pixels[(y*(2*max_x+200)+x)*4 + 2] = imageL.getPixelsPtr()[((max_y-y)*max_x+x)*4+2]; // B?
pixels[(y*(2*max_x+200)+x)*4 + 3] = 255; // A?
}
for(int x=1024;x<1224;x++){
pixels[(y*(2*max_x+200)+x)*4] = 0; // R?
pixels[(y*(2*max_x+200)+x)*4 + 1] = 0; // G?
pixels[(y*(2*max_x+200)+x)*4 + 2] = 0; // B?
pixels[(y*(2*max_x+200)+x)*4 + 3] = 255; // A?
}
for(int x = (max_x+200); x < (2*max_x+200); x++)
{
pixels[(y*(2*max_x+200)+x)*4] = imageR.getPixelsPtr()[((max_y-y)*max_x+x-(max_x+200))*4]; // R?
pixels[(y*(2*max_x+200)+x)*4 + 1] = imageR.getPixelsPtr()[((max_y-y)*max_x+x-(max_x+200))*4+1]; // G?
pixels[(y*(2*max_x+200)+x)*4 + 2] = imageR.getPixelsPtr()[((max_y-y)*max_x+x-(max_x+200))*4+2]; // B?
pixels[(y*(2*max_x+200)+x)*4 + 3] = 255; // A?
}
}
window.clear();
image.create(2248, 544, pixels);
texture.create(2248, 544);
texture.update(image);
sf::Sprite sprite;
sprite.setTexture(texture);
window.draw(sprite);
window.display();
delete pixels;
}
if(gui.goHomeCount==12){
goHomeMsg.body_part=0;
goHomeMsg.robot_side=0;
go_home_pub.publish(goHomeMsg);
std::cout<<"just published goHome LEFT_ARM"<<std::endl;
for(int i=0;i<6;i++){
ros::spinOnce();
loop_rate.sleep();
}
goHomeMsg.robot_side=1;
go_home_pub.publish(goHomeMsg);
std::cout<<"just published goHome RIGHT_ARM"<<std::endl;
for(int i=0;i<6;i++){
ros::spinOnce();
loop_rate.sleep();
}
goHomeMsg.body_part=1;
go_home_pub.publish(goHomeMsg);
std::cout<<"just published goHome TORSO"<<std::endl;
for(int i=0;i<6;i++){
ros::spinOnce();
loop_rate.sleep();
}
goHomeMsg.body_part=2;
go_home_pub.publish(goHomeMsg);
std::cout<<"just published goHome PELVIS"<<std::endl;
gui.startGoingHome=false;
gui.goHomeCount--;
ros::spinOnce();
loop_rate.sleep();
continue;
/*
moveSpeedOverRide=3;
arm_controller.publish(moveArm(JRIGHT,stateTracker.getArmHomeAngles()));
arm_controller.publish(moveArm(JLEFT,stateTracker.getArmHomeAngles()));
chest_controller.publish(moveChest(stateTracker.getChestHomeAngles()));
neck_controller.publish(moveHead(stateTracker.getNeckHomeAngles()));
pelvis_height_controller.publish(movePelvisHeight(stateTracker.getPelvisHeightHomeAngles()));
moveSpeedOverRide=-1;*/
}
if(!more || justChanged || (gui.goHomeCount>0 && gui.goHomeCount<12)){
//std::cout<<gui.goHomeCount<<" "<<more<<" "<<justChanged<<std::endl;
if(justChanged && more)
justChanged=false;
/*
ihmc_msgs::GoHomeRosMessage msg;
msg.trajectory_time=3;
msg.unique_id=5;
go_home_pub.publish(msg);
msg.robot_side=1;
go_home_pub.publish(msg);*/
if(gui.goHomeCount>0)
gui.goHomeCount--;
ros::spinOnce();
loop_rate.sleep();
continue;
}
std::vector<double> out;
out.push_back((tp.tv_sec * 1000 + tp.tv_usec / 1000)-end);//oh wow, this is terrible programming practice... dang man
if(end!=0)
gui.record("*: ",out);
//gui.gesture==0 acts as an unlock feature
if(gui.isRightArmOn && gui.gesture==1)
arm_controller.publish(moveArm(JRIGHT,stateTracker.getRightArmAngles()));
else if(gui.isRightArmOn && gui.gesture!=1 && false){
moveSpeedOverRide=3;
arm_controller.publish(moveArm(JRIGHT,stateTracker.getArmHomeAngles()));
moveSpeedOverRide=-1;
moveArm(JRIGHT,stateTracker.getRightArmAngles(),false);
}
if(gui.isLeftArmOn && gui.gesture==1)
arm_controller.publish(moveArm(JLEFT,stateTracker.getLeftArmAngles()));
else if(gui.isLeftArmOn && gui.gesture!=1 && false){
moveSpeedOverRide=3;
arm_controller.publish(moveArm(JLEFT,stateTracker.getArmHomeAngles()));
moveSpeedOverRide=-1;
//moveArm(JLEFT,stateTracker.getLeftArmAngles(),false);
}
if(gui.isChestOn && gui.gesture==1)
chest_controller.publish(moveChest(stateTracker.getChestAngles()));
else if(gui.isChestOn && gui.gesture!=1 && false){
moveSpeedOverRide=3;
chest_controller.publish(moveChest(stateTracker.getChestHomeAngles()));
moveSpeedOverRide=-1;
//moveChest(stateTracker.getChestAngles(),false);
}
if(gui.isHeadOn && gui.gesture==1)
neck_controller.publish(moveHead(stateTracker.getNeckAngles()));
else if(gui.isHeadOn && gui.gesture!=1 && false){
moveSpeedOverRide=3;
neck_controller.publish(moveHead(stateTracker.getNeckHomeAngles()));
moveSpeedOverRide=-1;
//moveHead(stateTracker.getNeckAngles(),false);
}
if(gui.isPelvisOn && gui.gesture==1)
pelvis_height_controller.publish(movePelvisHeight(stateTracker.getStandingHeight()));
else if(gui.isPelvisOn && gui.gesture!=1 && false){
moveSpeedOverRide=3;
pelvis_height_controller.publish(movePelvisHeight(stateTracker.getPelvisHeightHomeAngles()));
moveSpeedOverRide=-1;
//movePelvisHeight(stateTracker.getStandingHeight(),false);
}
ros::spinOnce();
gettimeofday(&tp, NULL);
long int start2 = tp.tv_sec * 1000 + tp.tv_usec / 1000;
loop_rate.sleep();
gettimeofday(&tp, NULL);
end = tp.tv_sec * 1000 + tp.tv_usec / 1000;
std::cout<<end-start2<<std::endl;
}
go_home_pub.publish(goHomeMsg);
goHomeMsg.robot_side=1;
go_home_pub.publish(goHomeMsg);
return 0;
}
ihmc_msgs::ArmTrajectoryRosMessage moveArm(int side,std::vector<double> angles){return moveArm(side,angles,true);}
/*
타이머 함수
*/
void timer(int input)
{
moveArm();
downArm();
glutTimerFunc(10, timer, 0);
}
