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