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