void fingerRoll(int del, int dir)
{
if(dir==CLOSE)
{
for(int i=NUM_FINGERS-1;i>=0;i--)
{
fingerControl(i,100,0, gripSpeed);
if(checkSerial())
return;
customDelay(del);
}
}
else
{
for(int i=0;i<NUM_FINGERS;i++)
{
fingerControl(i,0,0, gripSpeed);
if(checkSerial())
return;
customDelay(del);
}
}
}
void getCmd(void)
{
char serialChar;
int buffCount = 0;
// while there's data in the Serial buffer
while(MYSERIAL.available() > 0)
{
customDelay(1); // allow time for buffer to free up
serialChar = MYSERIAL.read();
serialCmd.cmdBuff[buffCount++] = serialChar;
}
// once the serial buffer is clear, set flag to indicate a received command to process and print the received command
if(buffCount > 0)
{
serialCmd.cmdBuff[buffCount] = 0; //terminate string
MYSERIAL.print("\n");
MYSERIAL.println(serialCmd.cmdBuff);
serialCmd.cmdsToGet++;
buffCount = 0; //clear buffer
}
}
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 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");
}
}
