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