void loop()
{
// Wait for command
if (commandAvailable)
{
parseCommand();
} // end commandAvailable
if( Serial.available() )
{
commandMode();
}
} // end loop
void waitBusy() {
readMode();
commandMode();
//D7 =Eingang
cbi(LCD_DATA_D,LCD_D7);
cbi(LCD_DATA,LCD_D7);
int busy;
do {
sbi(LCD_CTRL,LCD_E);
STROBE_DELAY();
busy=bit_is_set(LCD_INPUT,LCD_D7);
cbi(LCD_CTRL,LCD_E);
strobe();
}while (busy);
//D7 = Ausgang
sbi(LCD_DATA_D,LCD_D7);
}
int modeCheck(int c,int recordSelected,int *cursorLeft,int *cursorArea, char *subject,Cursor *cursor,Area rArea,Record **hovered,struct RecordList **activeBuffer, struct RecordList *RLBuffer) {
int redraw = FALSE;
if(commandMode(*cursorArea)) {
if(KEY_MODE_RECORDS(c) || KEY_MODE_ESCAPE(c)) {
redraw = check_record(cursorArea,hovered,activeBuffer,RLBuffer);
}
else if (KEY_MODE_ADD(c)) {
redraw = init_add(cursorArea,cursor);
}
else if (KEY_MODE_SEARCH(c)){
redraw = init_search(subject,cursor,cursorArea);
}
else if (KEY_MODE_EDIT(c)) {
if(*hovered) redraw = init_edit(recordSelected,cursorLeft,cursorArea,cursor,rArea,*hovered,*activeBuffer);
} else if(KEY_MODE_DELETE(c)) {
if(*hovered) redraw = init_delete(recordSelected,cursorArea,*hovered,*activeBuffer);
}
}
return redraw;
}
/** Called when command mode is active.
*
* First pressed key is mapped to first layer defined and evaluated. Use only a-z and 0-9 for commands, others exit this mode.
*
* @todo: leave automatically on unknown command or timeout, or signal mode through leds.
*
*/
void handleCommand(void) {
if(!commandMode())
return;
if(activeKeys.keycnt==0)
return;
struct Key k=activeKeys.keys[0];
uint8_t hid = getKeyCode(k.row, k.col, 0);
if(subcmd) {
handleSubCmd(k);
return;
}
clearActiveKeys();
clearRowData();
// Char with Meaning:
// A:ASCIIPrint, B:BootL, C:PrintConfig, G:GeoArea, H:HardwarePC/Mac, L: SwitchLayout, M:Macro, P:PrintLayout, Q:QuitCommand, T: Trackpoint
// Char without Meaning:
// O:MouseMode, R:PrintKeyHID
switch(hid) {
#ifdef PINKYDROP
case HID_D:
g_pinkydrop = g_pinkydrop ? 0 : 1;
printf("\nPinkydrop %d", g_pinkydrop);
eeprom_write_byte(&ee_pinkyDrop, g_pinkydrop);
setCommandMode(false);
break;
#endif
case HID_V:
printf("\nAdNW %s", FW_VERSION);
setCommandMode(false);
break;
case HID_Q:
case HID_ESC:
printf("\nLeaving command mode::");
setCommandMode(false);
break;
case HID_B:
printf("\nBootloader::");
jump_bootloader();
break;
/*
case HID_P:
// Print Layout: one layer per press on key 'p'
printLayout(layer);
layer=(layer+1)%LAYERS;
if(layer==0)
setCommandMode(false);
break;
*/
#ifdef MOUSE_HAS_SCROLL_WHEELS
case HID_T:
printf("\nTrackpoint:");
tp_id();
setCommandMode(false);
break;
#endif
case HID_L:
g_alternateLayer = g_alternateLayer ? 0 : 1;
eeprom_write_byte(&ee_alternateLayer,g_alternateLayer);
printf("\nAlternate layer %s", g_alternateLayer ? "selected." : "off.");
setCommandMode(false);
break;
#ifdef PS2MOUSE
case HID_M:
g_mouse_enabled = g_mouse_enabled > 0 ? 0 : 1;
printf("\nMouse %sabled", g_mouse_enabled ? "en" : "dis");
setCommandMode(false);
break;
#endif
case HID_X:
subcmd=SUB_MACRO;
printf("Macro mode true\n");
break;
case HID_R:
printf("Macro recording\n");
subcmd=SUB_MACRO_REC;
break;
case HID_H:
subcmd=SUB_PASSHASH;
break;
default:
printf("\nUnknown command.");
break;
}
}
/** Called when command mode is active.
*
* First pressed key is mapped to first layer defined and evaluated. Use only a-z and 0-9 for commands, others exit this mode.
*
* @todo: leave automatically on unknown command or timeout, or signal mode through leds.
*
*/
void handleCommand(void) {
if(!commandMode())
return;
if(activeKeys.keycnt==0)
return;
struct Key k=activeKeys.keys[0];
uint8_t hid = getKeyCode(k.row, k.col, 0);
if(subcmd) {
handleSubCmd(k);
return;
}
clearActiveKeys();
clearRowData();
// Char with Meaning:
// A:ASCIIPrint, B:BootL, C:PrintConfig, G:GeoArea, H:HardwarePC/Mac, L: SwitchLayout, M:Macro, P:PrintLayout, Q:QuitCommand, T: Trackpoint
// Char without Meaning:
// O:MouseMode, R:PrintKeyHID
switch(hid) {
case HID_D:
g_pinkydrop = !g_pinkydrop;
printf("\nPinkydrop %d", g_pinkydrop);
setCommandMode(false);
break;
case HID_V:
printf("\nAdNW %s", FW_VERSION);
setCommandMode(false);
break;
case HID_C:
PrintConfiguration();
break;
case HID_G:
// G:GeoArea umschalten
printf("\nG:GeoArea::");
subcmd=SUB_GEOAREA;
break;
case HID_H:
// HardwarePC/Mac umschalten
printf("\nHardwarePC/Mac::");
subcmd=SUB_PC_MAC;
break;
case HID_Q:
case HID_ESC:
printf("\nLeaving command mode::");
setCommandMode(false);
break;
case HID_B:
printf("\nBootloader::");
jump_bootloader();
break;
case HID_P:
// Print Layout: one layer per press on key 'p'
printLayout(layer);
layer=(layer+1)%LAYERS;
if(layer==0)
setCommandMode(false);
break;
#ifdef MOUSE_HAS_SCROLL_WHEELS
case HID_T:
printf("\nTrackpoint:");
tp_id();
setCommandMode(false);
break;
#endif
case HID_L:
g_alternateLayer=!g_alternateLayer;
printf("\nAlternate layer %s", g_alternateLayer ? "selected." : "off.");
setCommandMode(false);
/*
// Layout umschalten
printf("\nSwitch layout::");
subcmd=SUB_LAYOUT;
*/
break;
case HID_A:
for(uint8_t i=32; i<255; ++i) {
if(i%16==0)
printf("\n %03d", i);
printf("%c ", (char)(i));
if(i==127)
i+=33;
}
setCommandMode(false);
break;
case HID_M:
g_mouse_enabled = g_mouse_enabled > 0 ? 0 : 1;
printf("\nMouse %sabled", g_mouse_enabled ? "en" : "dis");
setCommandMode(false);
break;
case HID_R:
// print HID code of pressed key
printf("\nHID code read active.");
subcmd=SUB_READ;
idx=0;
break;
case HID_X:
subcmd=SUB_MACRO;
printf("Macro mode true\n");
break;
default:
printf("\nUnknown command.");
break;
}
}
int main(int argc, char **argv)
{
Aria::init();
ros::init(argc, argv, "teleoparm");
ArLog::init(ArLog::StdErr, ArLog::Normal);
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
//ArRobot robot;
ArRobotConnector robotConnector(&parser, &robot);
if(!robotConnector.connectRobot())
{
ArLog::log(ArLog::Terse, "terabotArmDemo: Could not connect to the robot.");
if(parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
//ArTerabotArm arm(&robot);
ArLog::log(ArLog::Normal, "terabotArmDemo: Connected to mobile robot.");
if(!arm.open())
{
ArLog::log(ArLog::Terse, "terabotArmDemo: Error opening serial connection to arm");
Aria::exit(1);
}
robot.runAsync(true);
arm.powerOn();
arm.reset();
arm.enable();
arm.setAllJointSpeeds(defaultJointSpeed);
ArLog::log(ArLog::Terse, "\nWARNING\n\nAbout to move the manipulator arm to the front of the robot in 5 seconds. Press Control-C to cancel and exit the program.\n...");
ArUtil::sleep(5000);
ArLog::log(ArLog::Terse, "Now moving the arm...");
// arm.moveArm(forwardReady);
//arm.moveArm(park);
ArUtil::sleep(500); // need to have read some data from the arm for key handler to work
robot.lock();
ArModeUnguardedTeleop unguardedTeleopMode(&robot, "unguarded teleop", 'u', 'U');
ArModeTeleop teleopMode(&robot, "teleop", 't', 'T');
ArModeLaser laserMode(&robot, "laser", 'l', 'L');
ArModeCommand commandMode(&robot, "direct robot commands", 'd', 'D');
ros::NodeHandle n;
sensor_msgs::JointState joints;
ros::Rate loop_rate(10);
ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("/joint_states", 50);//50 1000
ros::Subscriber arm_sub_ = n.subscribe<sensor_msgs::JointState>("/joint_states", 10, move_arm);//it was 100
float positions[5];
// float park1 [5]= {50, -95,-50, -149, 0};
float park1 [5]= {149, -94, 155, -149, 0};//this is th real park
float initial [5]= {-30.0, 0.0, 0.0, 0.0, 0.0};
// std::cout<<"Gripper Status before:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status before:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status before:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status before:"<<arm.getGripperStatus()<<"\n";
// arm.grip(-100);
// std::cout<<"Gripper Status after:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status after:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status after:"<<arm.getGripperStatus()<<"\n";
// std::cout<<"Gripper Status after:"<<arm.getGripperStatus()<<"\n";
//*********testing**************************
// arm.moveArm(start);
//arm.closeGripper();
// arm.moveArm(park1);
arm.moveArm(forwardReady);// used when the manual control GUI is used
ArUtil::sleep(20000);// time for the non blocking predefined movements to take place
//******************************************
while(ros::ok())
{
joints.name.push_back("j1");
joints.name.push_back("j2");
joints.name.push_back("j3");
joints.name.push_back("j4");
joints.name.push_back("j5");
// why is there no joint
//joints.position.push_back(11);//<<
joints.position.clear();
joints.position.push_back(positions[0]);
joints.position.push_back(positions[1]);
joints.position.push_back(positions[2]);
joints.position.push_back(positions[3]);
joints.position.push_back(positions[4]);
joint_pub.publish(joints);
//arm.getJointStatus(&j1,&j2,&j3,&j4,&j5);
arm.getArmPos(positions);
// std::cout<<"j1:"<<j1<<"j2:"<<j2<<"j3:"<<j3<<"j4:"<<j4<<"j5:"<<j5<<"\n"; fflush(stdout);
std::cout<<"position1:"<<positions[0]<<"position2:"<<positions[1]<<"position3:"<<positions[2]<<"position4:"<<positions[3]<<"position5:"<<positions[4]<<"\n"; fflush(stdout);
//ros::spin();
ros::spinOnce();
loop_rate.sleep();
// std::cout<<"loopend\n";
}
robot.enableMotors();
robot.unlock();
robot.waitForRunExit();
ArLog::log(ArLog::Normal, "terabotArmDemo: Either mobile robot or arm disconnected. Exiting.");
Aria::exit(0);
}
