示例#1
0
文件: Demo.cpp 项目: Kwell/Artichoke
void demoMode(void)
{
	MYSERIAL.println("Demo starting");
	
	printGrip(FIST_GRIP,OPEN);
	gripMovement(FIST_GRIP,BLANK,OPEN, gripSpeed);
 if(checkSerial())
 return;
	customDelay(gripDuration);
	
	printGrip(FIST_GRIP,CLOSE);
	gripMovement(FIST_GRIP,BLANK,CLOSE, gripSpeed);
  if(checkSerial())
  return;
	customDelay(gripDuration);
	
	printGrip(FIST_GRIP,OPEN);
	gripMovement(FIST_GRIP,BLANK,OPEN, gripSpeed);
  if(checkSerial())
  return;
	customDelay(gripDuration);
	
	printGrip(FINGER_ROLL,CLOSE);
	fingerRoll(300,CLOSE);
	if (checkSerial())
		return;
	customDelay(gripDuration / 4);
	
	printGrip(FINGER_ROLL,OPEN);
	fingerRoll(300,OPEN);
	if (checkSerial())
		return;
	customDelay(gripDuration / 2);
	
	printGrip(FINGER_ROLL,CLOSE);
	fingerRoll(300,CLOSE);
  if(checkSerial())
  return;
	customDelay(gripDuration/2);
	
	printGrip(FINGER_ROLL,OPEN);
	fingerRoll(300,OPEN);
	
	printGrip(FINGER_ROLL,CLOSE);
	fingerRoll(300,CLOSE);
  if(checkSerial())
  return;
	customDelay(gripDuration/4);
	
	printGrip(FIST_GRIP,OPEN);
	gripMovement(FIST_GRIP,BLANK,OPEN, gripSpeed);
  if(checkSerial())
  return;
	customDelay(gripDuration);

	for(int i=0;i<NUM_GRIPS;i++)
	{
		printGrip(i,CLOSE);
		gripMovement(i,BLANK,CLOSE, gripSpeed);
    if(checkSerial())
    return;
		customDelay(gripDuration);
		
		printGrip(i,OPEN);
		gripMovement(i,BLANK,OPEN, gripSpeed);
    if(checkSerial())
    return;
		customDelay(gripDuration);
	}
	
	#ifdef TRIPOD_GRIP
	printGrip(TRIPOD_GRIP,CLOSE);
	gripMovement(TRIPOD_GRIP,BLANK,CLOSE, gripSpeed);
  if(checkSerial())
  return;
	customDelay(gripDuration);
	
	printGrip(TRIPOD_GRIP,OPEN);
	gripMovement(TRIPOD_GRIP,BLANK,OPEN, gripSpeed);
  if(checkSerial())
  return;
	customDelay(gripDuration);
	#endif
	
	printGrip(PINCH_GRIP,CLOSE);
	gripMovement(PINCH_GRIP,BLANK,CLOSE, gripSpeed);
  if(checkSerial())
  return;
	customDelay(gripDuration);
	
	printGrip(FIST_GRIP,OPEN);
	gripMovement(FIST_GRIP,BLANK,OPEN, gripSpeed);
  if(checkSerial())
  return;
	customDelay(gripDuration);
	
	printGrip(FINGER_ROLL,CLOSE);
	fingerRoll(430,CLOSE);
	customDelay(gripDuration/2);
	
	printGrip(FIST_GRIP,OPEN);
	gripMovement(FIST_GRIP,BLANK,OPEN, gripSpeed);
  if(checkSerial())
  return;
	
	if(!advancedSettings.demoFlag)
		demoFlag = 0;

	MYSERIAL.println("Demo complete");
	
	// if demo mode is enabled from start up
	if(advancedSettings.demoFlag)
	{
		MYSERIAL.println("\nStart up demo mode ON");
		MYSERIAL.println("Enter A0 to disable this mode");
	}
}
示例#2
0
void HANDleMain(void)
{
	static bool initialised = false;		// initialised flag
	static float HANDlePos = 0;				// starting pos for the HANDle
	
	float yPos, increment;
	
	// configuration
	if(!initialised)
	{
		Wire.begin();
		wii.init();

		MYSERIAL_PRINTLN_PGM("Calibrating mid position of joystick");
		wii.poll();
		customDelay(100);
		wii.poll();
		wii.calibrate();  // calibration (needs a poll before hand to work)
		MYSERIAL_PRINTLN_PGM("Complete");
		initialised = true;
	}
	
	wii.poll();
	
	if(wii.buttonC()&&wii.buttonZ())				// if both buttons are pressed at the same time, calibrate mid position of the joystick
	{
		MYSERIAL_PRINTLN_PGM("Calibrating mid position of joystick");
		wii.poll();
		customDelay(100);
		wii.poll();
		wii.calibrate();  // calibration (needs a poll before hand to work)
	}
	
	if(wii.buttonC())							// if one button press, change grip on a button press 
	{
		currentGrip ++;
		if(currentGrip >= NUM_GRIPS)
			currentGrip = 0;

		MYSERIAL_PRINT_PGM("Grip number ");
		MYSERIAL_PRINT(currentGrip);
		MYSERIAL_PRINT_PGM("  ");
		MYSERIAL_PRINTLN(textString.grips[currentGrip]);
		
		customDelay(200);
	}
	if(wii.buttonZ())
	{
		currentGrip --;
		if(currentGrip < 0)
			currentGrip = NUM_GRIPS-1;
	
			MYSERIAL_PRINT_PGM("Grip number ");
			MYSERIAL_PRINT(currentGrip);
			MYSERIAL_PRINT_PGM("  ");
			MYSERIAL_PRINTLN(textString.grips[currentGrip]);
		
		customDelay(200);
	}	
	
	yPos = wii.joyY();
	
	if(HANDleSerialFlag)
	{
		MYSERIAL_PRINT_PGM("Grip ");
		MYSERIAL_PRINT(currentGrip);
		MYSERIAL_PRINT_PGM("  \tjoyPos ");
		MYSERIAL_PRINT(yPos);
	}
	
	yPos = constrain(yPos,-90,90);
	yPos = (float) yPos/(180/PI);			// deg to rad

	increment = 2*sin(yPos);
	
	if(abs(yPos) <= MID_TOLERANCE)
	{
		increment = 0;
		yPos = 0;
	}
	
	HANDlePos += increment;
	HANDlePos = constrain(HANDlePos,0,100);
		
	gripMovement(currentGrip,HANDlePos);
	
	
	
	if(HANDleSerialFlag)
	{
		MYSERIAL_PRINT_PGM("  \tHANDle pos ");
		MYSERIAL_PRINT(yPos);
		MYSERIAL_PRINT_PGM("  \tHand pos ");
		MYSERIAL_PRINTLN((int) HANDlePos);
	}
	else
	{
		customDelay(10);		// delay to counteract time spent printing to serial (required as Nunchuck IC is slow)
	}

}
示例#3
0
void manageSerialSettings(void)
{
	if(serialCmd.fingerNum != BLANK)
	{
		MYSERIAL.print("\n");
		MYSERIAL.print("Finger ");
		MYSERIAL.println(serialCmd.fingerNum);
		
		MYSERIAL.print("Direction ");
		if(serialCmd.direction != BLANK)
			MYSERIAL.println(textString.open_close[serialCmd.direction]);
		else
		{	
			int fingerDir = !finger[serialCmd.fingerNum].readDir();
			MYSERIAL.println(textString.open_close[fingerDir]);
		}
		
		MYSERIAL.print("Stop Position ");
		if(serialCmd.stopPos != BLANK)
			MYSERIAL.println(serialCmd.stopPos);
		else
		MYSERIAL.println("None");
		
		MYSERIAL.print("Speed ");
		if(serialCmd.speed != BLANK)
			MYSERIAL.println(serialCmd.speed);
		else
			MYSERIAL.println(finger[serialCmd.fingerNum].readTargetSpeed());

		fingerControl(serialCmd.fingerNum, serialCmd.stopPos, serialCmd.direction, serialCmd.speed);
	}
	else if(serialCmd.gripNum != BLANK)
	{
		MYSERIAL.print("Grip ");
		MYSERIAL.println(textString.grips[serialCmd.gripNum]);
		
		MYSERIAL.print("Direction ");
		if(serialCmd.direction != BLANK)
			MYSERIAL.println(textString.open_close[serialCmd.direction]);
		else
			MYSERIAL.println("None");
		
		MYSERIAL.print("Stop Position ");
		if(serialCmd.stopPos != BLANK)
			MYSERIAL.println(serialCmd.stopPos);
		else
			MYSERIAL.println((100 * serialCmd.direction));
			
		MYSERIAL.print("Speed ");
		if(serialCmd.speed != BLANK)
			MYSERIAL.println(serialCmd.speed);
		else
		{
			int fingerToRead = (serialCmd.gripNum==THUMBSUP_GRIP)?FINGER0:FINGER1;
			MYSERIAL.println(finger[fingerToRead].readTargetSpeed());
		}
			
		MYSERIAL.print("\n");

		gripMovement(serialCmd.gripNum, serialCmd.stopPos, serialCmd.direction, serialCmd.speed);
	}
  
	else if(serialCmd.instructionsFlag != BLANK)
	{
		printInstructions();
	}
  
	else if(serialCmd.advancedFlag != BLANK)
	{
		switch(serialCmd.advancedFlag)
		{
			case 0: // demo mode
				advancedSettings.demoFlag = !advancedSettings.demoFlag;   // toggle flag
				demoFlag = advancedSettings.demoFlag;      
				MYSERIAL.print("Demo mode toggled ");
				MYSERIAL.println(textString.off_on[advancedSettings.demoFlag]); // print ON/OFF
				break;
			case 1: // serial instructions
				advancedSettings.instructionsFlag = !advancedSettings.instructionsFlag; // toggle flag
				MYSERIAL.print("Initial serial instructions toggled ");
				MYSERIAL.println(textString.off_on[advancedSettings.instructionsFlag]); // print ON/OFF
				break;
			case 2: // muscle graph
				advancedSettings.muscleGraphFlag = !advancedSettings.muscleGraphFlag; // toggle flag
				MYSERIAL.print("Muscle graph mode toggled ");
				MYSERIAL.println(textString.off_on[advancedSettings.muscleGraphFlag]);  // print ON/OFF
				break;
			case 3: // motor enable/disable
				advancedSettings.motorEnable = !advancedSettings.motorEnable;   // toggle flag
				MYSERIAL.print("Motors ");
				
				MYSERIAL.println(textString.disabled_enabled[advancedSettings.motorEnable]);  // print Disabled/Enabled
				for(int i=0;i<NUM_FINGERS;i++)
				{
					if(advancedSettings.motorEnable)
						finger[i].enableMotor();
					else
						finger[i].disableMotor();
				}
				break;
			case 5: // motors emergency stop
				stopMotors();
				MYSERIAL.println("Stop Motors");
				break;
			case 10:
				advancedSettings.researchFlag = !advancedSettings.researchFlag;
				MYSERIAL.print("Research mode 1 toggled ");
				MYSERIAL.println(textString.off_on[advancedSettings.researchFlag]);
				break;
#ifdef HANDLE_EN
			case 11:
				advancedSettings.HANDle_en = !advancedSettings.HANDle_en;
				MYSERIAL.print("HANDle mode ");
				MYSERIAL.println(textString.disabled_enabled[advancedSettings.HANDle_en]);
				EEPROM_writeStruct(ADVANCED_CTRL_LOC, advancedSettings);
				break;
			case 12:
				toggleHANDleSerial();
				break;
#endif
			default:
				break;
		}
		EEPROM_writeStruct(ADVANCED_CTRL_LOC,advancedSettings);   // save settings to EEPROM
	}
  
	else if(serialCmd.muscleCtrlFlag != BLANK)
	{
		switch(serialCmd.muscleCtrlFlag)
		{
			case -1:            // default
				break;
			case 0:             // muscle control off
				advancedSettings.muscleCtrlFlag = 0;
				MYSERIAL.println("Muscle control OFF ");
				digitalWrite(LED_KNUCKLE,LOW);  // turn off muscle control LED in knuckle
				break;
			case 1:             // standard muscle control
				advancedSettings.muscleCtrlFlag = 1;
				MYSERIAL.println("Standard muscle control ON");
				digitalWrite(LED_KNUCKLE,HIGH);   // turn on muscle control LED in knuckle
				break;
			case 2:             // position muscle control
				advancedSettings.muscleCtrlFlag = 2;
				MYSERIAL.println("Muscle position control ON");
				digitalWrite(LED_KNUCKLE,HIGH);   // turn on muscle control LED in knuckle
				break;
			case 3:             // print muscle readings
				if(advancedSettings.muscleCtrlFlag != 0)
				{
					printADCvals = !printADCvals;
					MYSERIAL.print("Display muscle readings ");
					MYSERIAL.println(textString.off_on[printADCvals]);     // print ON/OFF
				}
				break;
			default:
				break;
		}
      
		EEPROM_writeStruct(ADVANCED_CTRL_LOC,advancedSettings);   // store muscle mode in EEPROM
	}
 
	else if(serialCmd.handFlag != BLANK)  // if 'H#'
	{
		if(serialCmd.handFlag != 0)
		{
			advancedSettings.handFlag = serialCmd.handFlag;
			IOconfig();             // reconfigure finger pins
			EEPROM_writeStruct(ADVANCED_CTRL_LOC,advancedSettings);    // store hand in EEPROM
		}
		MYSERIAL.print("Hand is ");
		MYSERIAL.println(textString.right_left[advancedSettings.handFlag-1]); // print which hand is entered
	}
 
	else if(serialCmd.demoFlag == 0) // if 'D'
	{
		demoFlag = 1;
		MYSERIAL.println("Demo Mode");
		MYSERIAL.println(" The hand is only responsive to serial commands\n at the end of each demo cycle");
	}
  
	else if(serialCmd.sensitivityAdjust != BLANK)
	{
		if(serialCmd.sensitivityAdjust != 0)
		{
			userSettings.sensitivityOffset = serialCmd.sensitivityAdjust;
			EEPROM_writeStruct(USER_SETTINGS_LOC,userSettings);
		}
		MYSERIAL.print("Muscle sensor sensitivity ");
		MYSERIAL.println(userSettings.sensitivityOffset);
	}
  
	else if(serialCmd.noiseFloor != BLANK)
	{
		runNoiseFloorCalc();
	}

	else if(serialCmd.holdTime != BLANK)
	{
		if(serialCmd.holdTime != 0)
		{
			userSettings.holdTime = serialCmd.holdTime;
			EEPROM_writeStruct(USER_SETTINGS_LOC,userSettings);
		}
		MYSERIAL.print("Grip change hold duration ");
		MYSERIAL.println(userSettings.holdTime);
	}

	// if research mode == 1, and no other command is recognised, use CSV string as target motor positions 
	else if (advancedSettings.researchFlag == 1)		// if 'A10'
	{
		researchMode_CSV_RX(serialCmd.cmdBuff);
	}

	clearAll();									// clear all serial commands (clear serialCmd.buffs)
}