//--------------------------------------------------------------
void testApp::beat() {
if (ofGetElapsedTimeMillis() < 100) {
return;
}
// do something that is visibly different from what you're currently doing
if (servoPosition > 0) {
moveServo(-60, true);
}else {
moveServo(60, true);
}
ofResetElapsedTimeCounter();
}
void setup()
{
for (int i = lls; i <= hv; i++) {
moveServo(i, 0);
servos[i].attach(ports[i]);
moveServo(i, 0);
}
for(int i = 0; i < 50; i++) {
Servo::refresh();
delay(50);
}
}
void main (void)
{
WDTCTL = WDTPW | WDTHOLD; // Stop watchdog timer
// Set 16MHz clock
BCSCTL1 = CALBC1_16MHZ;
DCOCTL = CALDCO_16MHZ;
// Startup Delay
_delay_cycles(2000);
// Initialize serial port
SERIAL_init();
// Enable interrupt
_EINT();
rxEchoPos = 0;
// Set the servo port to digital output
P2DIR |= BIT2;
int deg; // servo rotation degree
// Main loop
while(1)
{
// load the target degree from the serial port
deg = checkSerial();
// if there is a new value
if(deg > -1)
{
// Move the servo
moveServo(deg);
}
__delay_cycles(1000);
}
}
bool steering::steer(LineSensor lineSensor, Servo motor, int speed) {
int treebeard = getAngle(lineSensor, motor.read(), speed);
if (treebeard == 314) {
return false;
} else if(treebeard == -314) {
setServo(STEERING_CENTER, motor);
return true;
}
moveServo(treebeard, motor);
//setServo(STEERING_CENTER - treebeard, motor);
/*string cheese
double dutch
char lizard
float rootbeer
long way_to_go
byte me!
short people
name cecil
enter the void
;oscapy*/
return true;
}
void testInput(char inChar){
switch(inChar){
case 'w': //Arm forward
moveServo(0, UP);
break;
case 's': //Arm back
moveServo(0,DOWN);
break;
case 'd': //Rotate CCW
moveServo(1, LEFT);
break;
case 'a': //Rotate CW
moveServo(1, RIGHT);
break;
case 'i': //Arm up
moveServo(2, UP);
break;
case 'k': //Arm down
moveServo(2, DOWN);
break;
case 'l': //Close claw
moveServo(3, UP);
break;
case 'j': //Open claw
moveServo(3, DOWN);
break;
case 'r': //Reset position
servoPosition[0] = 1500;
servoPosition[1] = 1500;
servoPosition[2] = 1500;
servoPosition[3] = 1500;
break;
default: //Not a valid Key, Display possible keys
messageOut("----- Invalid Key -----",23);
messageOut("Left/Right: 'a' + 'd'",21);
messageOut("Up/Down: 'i' + 'k'",18);
messageOut("Forward/Back: 'w' + 's'",23);
messageOut("Open/Close: 'l' + 'j'",21);
messageOut("Reset Position: 'r'",19);
messageOut("",0); //Newline
break;
}//switch
}//testInput()
void bank(int direction)
{
int lfPos = servoPos[lfs];
int rfPos = servoPos[rfs];
int lfMoveTo = leftBankAngles[direction + 1];
int rfMoveTo = rightBankAngles[direction + 1];
int bankDone = 0;
int lfDir, rfDir;
if (lfPos > lfMoveTo)
lfDir = -1;
else
lfDir = 1;
if (rfPos > rfMoveTo)
rfDir = -1;
else
rfDir = 1;
while (!bankDone) {
bankDone = 1;
if (lfMoveTo + leftSpeeds[direction+1] - 1 < lfPos || lfMoveTo - leftSpeeds[direction+1] + 1 > lfPos) {
moveServo(lfs, lfPos+lfDir*leftSpeeds[direction+1]);
bankDone = 0;
}
if (rfMoveTo + rightSpeeds[direction+1] - 1 < rfPos || rfMoveTo - rightSpeeds[direction+1] + 1 > rfPos) {
moveServo(rfs, rfPos+rfDir*rightSpeeds[direction+1]);
bankDone = 0;
}
lfPos = servoPos[lfs];
rfPos = servoPos[rfs];
Servo::refresh();
}
}
void moveLegs(int direction)
{
int llPos = servoPos[lls];
int rlPos = servoPos[rls];
int llMoveTo = leftLegAngles[direction + 1];
int rlMoveTo = rightLegAngles[direction + 1];
int walkDone = 0;
int llDir, rlDir;
if (llPos > llMoveTo)
llDir = -1;
else
llDir = 1;
if (rlPos > rlMoveTo)
rlDir = -1;
else
rlDir = 1;
while (!walkDone) {
walkDone = 1;
if (llMoveTo + legSpeed - 1 < llPos || llMoveTo - legSpeed + 1 > llPos) {
moveServo(lls, llPos+llDir*legSpeed);
walkDone = 0;
}
if (rlMoveTo + legSpeed - 1 < rlPos || rlMoveTo - legSpeed + 1 > rlPos) {
moveServo(rls, rlPos+rlDir*legSpeed);
walkDone = 0;
}
llPos = servoPos[lls];
rlPos = servoPos[rls];
Servo::refresh();
}
}
void steering::steer(LineSensor lineSensor, int size, Servo motor, int speed) {
int* data = new int [size];
int priority;
int angle;
lineSensor.update();
lineSensor.getCurrent(data);
angle = getAngle(data, size);
moveServo(angle, motor);
angle = motor.read() - 90;
drive(angle, speed);
delete [] data;
return;
}
task main()
{
nMotorEncoder[AMotor] = 0; //resets the motor encoder
nMotorEncoderTarget[AMotor] = -rotationCount; //set the target stopping position
motor[AMotor] = - powerPercentage; //sets motor to power at powerPercentage
moveServo(armServoMovementTime, armServoPower);
servo[ClawServo] = ServoValue[ClawServo] + clawServoPosition;
while (nMotorRunState[AMotor] != runStateIdle) //while the encoder wheel turns
{
//waits for the motor to stop
}
motor[AMotor] = 0; //turns the motor
wait1Msec(1000);
}
//--------------------------------------------------------------
void testApp::keyPressed(int key){
/////////////
// ofxBeatTracking keyPressed
//////////
// 真偽値を反転させて画面表示をコントロール
if(key == 'd'){
drumVisible = !drumVisible;
}
if(key == 's'){
snareVisible = !snareVisible;
}
if(key == 'h'){
hihatVisible = !hihatVisible;
}
/////////////
// Custom Apache Key handling
//////////
switch (key) {
case OF_KEY_RIGHT:
// turn LED on
ard.sendDigital(18, ARD_HIGH); // pin 20 if using StandardFirmata from Arduino 0022 or older
if (ofGetKeyPressed(OF_KEY_SHIFT)) {
// rotate servo head to 180 degrees
moveServo(180, false);
cout << "keyPressed(OF_KEY_RIGHT); // with OF_KEY_SHIFT pressed" << endl;
} else {
// rotate servo incrementally every time key is pressed
cout << "keyPressed(OF_KEY_RIGHT);" << endl;
moveServo(30, true);
}
break;
case OF_KEY_LEFT:
// turn LED off
ard.sendDigital(18, ARD_LOW); // pin 20 if using StandardFirmata from Arduino 0022 or older
if (ofGetKeyPressed(OF_KEY_SHIFT)) {
// rotate servo head to 0 degrees
moveServo(0, false);
cout << "keyPressed(OF_KEY_LEFT); // with OF_KEY_SHIFT pressed" << endl;
} else {
// rotate servo incrementally every time key is pressed
moveServo(-30, true);
cout << "keyPressed(OF_KEY_LEFT);" << endl;
}
break;
case 32:
beat();
cout << "keyPressed(" ");" << endl;
break;
case OF_KEY_SHIFT:
cout << "keyPressed(OF_KEY_SHIFT);" << endl;
break;
default:
cout << "keyPressed(key); // key = " << key << endl;
break;
}
}
void steering::steer(LineSensor lineSensor, Servo motor, int speed) {
int angle = getAngle(lineSensor) / REDUCE;
moveServo(angle, motor);
return;
}
task main()
{
int threshold = 15; //to avoid unnecessary movement
while(true) //infinite loop
{
getJoystickSettings(joystick); //retrieves data from the joystick
if(abs(joystick.joy1_y1) > threshold)
{
motor[green] = joystick.joy1_y1 /2; //y1 controller moves motorD
}
else
{
motor[green] = 0;
}
if(abs(joystick.joy1_y2) > threshold)
{
motor[red] = joystick.joy1_y2 /2; //y2 controller moves motorE
}
else
{
motor[red] = 0;
}
if(joy2Btn(5) == 1 || joy1Btn(5) == 1) //If Joy2-Button5 || Joy1-Button5 is pressed:
{
moveServo (127) ;
}
if(joy2Btn(6) == 1 || joy1Btn(6) == 1) //If Joy2-Button6 || Joy1-Button6 is pressed:
{
moveServo (0);
}
if(joy2Btn(7) == 1 || joy1Btn(7) == 1) //If Joy2-Button7 || Joy1-Button7 is pressed:
{
motor[motorA] = -60;
}
else if(joy2Btn(8) == 1 || joy1Btn(8) == 1) //If Joy2-Button8 || Joy1-Button8 is pressed:
{
motor[motorA] = 60;
}
else
{motor[motorA] = 0;
}
if(abs(joystick.joy2_y2) > threshold)
{
motor[blue] = joystick.joy2_y2/2; //y2 controller moves harvester
}
else
{
motor[blue] = 0;
}
}
}
//this function will bring the arm up (regardless of the claw position)
void clawClose() {
printf("claw close");
moveServo(CLAW_SERVO, CLAW_CLOSE_POS);
}
//this function will bring the arm up (regardless of the claw position)
void clawOpen() {
printf("claw open");
moveServo(CLAW_SERVO, CLAW_OPEN_POS);
}
//this function will bring the arm up (regardless of the claw position)
void armDown() {
printf("moving arm down");
moveServo(ARM_SERVO, ARM_DOWN_POS);
}
//this function will bring the arm up (regardless of the claw position)
void armUpHalfWay() {
printf("moving arm up 1/2 way\n");
moveServo(ARM_SERVO, ARM_UP_HALFWAY_POS);
}
//this function will bring the arm up (regardless of the claw position)
void armUp() {
printf("moving arm up\n");
moveServo(ARM_SERVO, ARM_UP_POS);
}
