Combination Report Einstein Robot



Download 10.84 Mb.
Page15/41
Date conversion08.07.2018
Size10.84 Mb.
1   ...   11   12   13   14   15   16   17   18   ...   41


Final Code including arm, head and base


#pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)

#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorNormal, openLoop)

#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorNormal, openLoop, reversed)

#pragma config(Motor, mtr_S1_C2_1, motorF, tmotorNormal, openLoop, encoder)

#pragma config(Motor, mtr_S1_C2_2, motorG, tmotorNormal, openLoop, encoder)

#pragma config(Servo, srvo_S1_C3_1, , tServoNormal)

#pragma config(Sensor, S3, MSHID, sensorI2CCustom9V)

//*!!Code automatically generated by 'ROBOTC' configuration wizard!!*//


/**************************************************************/

/* */


/* Program Name: Robot Team 1 Arm/Base/Head code */

/* */


/* Authors: : David Gaskin(d.b.gaskin@gmail.com), */

/* Nathan Makowski(nathan.makowski@gmail.com), */

/* Caroline Stepnowski(kestpur@comcast.net), */

/* Roger Bingham(roger.bingham@gmail.com), */

/* Matt Blackmore(blackmom@pdx.edu), */

/* Shaun Ochsner(scochsner@gmail.com), */

/* Jacob Furniss(jacob.furniss@gmail.com). */

/* */


/* */

/* This program was written for the intent of educational use.*/

/* Much of this code was used from samples provided by */

/* mindsensors.com. For full samples, see: */

/* http://rdpartyrobotcdr.sourceforge.net/documentation/ */

/* files.html */

/* http://www.mindsensors.com/index.php?module=documents& */

/* JAS_Doc umentManager_op=viewDocument&JAS_Document_id=13 */

/* */

/**************************************************************/


const ubyte Addr = 0x02;

const tSensors SensorPort = S2; // Connect PSPNX sensor to this port!!

// S indicates port on the NXT brick

#include "PSP-Nx-lib.c"

#include "common.h"

#include "MSHID-driver.h"


int nLeftButton = 0;

int nRightButton = 0;

int nEnterButton = 0;

int nExitButton = 0;

task

main ()


{

// motors to arm and base

//Base motors

int powerD = 0;

int powerE = 0;

//Arm motors

int powerF = 0;

int powerG = 0;

// joystick indicators

int d_left_X = 0;

int d_left_Y = 0;

int d_right_X = 0;

int d_right_Y = 0;

int tri, squ, cir, cro, a, b, c, d;

psp currState;




//

// Note: program cannot be terminated if we hijack the 'exit' button. So



// there has to be that will return buttons to system control! We'll use a

// triple click

nNxtExitClicks = 3;
nI2CBytesReady[SensorPort] = 0;
SensorType[SensorPort] = sensorI2CMuxController;

wait10Msec (100);


while ( true )

{

wait1Msec (5);


PSP_ReadButtonState(SensorPort, Addr, currState);

// getting pot states from joystick

d_left_X = (int)currState.l_j_x;

d_left_Y = (int)currState.l_j_y;

// fixing reversal problem

// Back left and back right were reveresed, so we

// implemented this fix.

if (d_left_Y <= 0)

{

powerD = d_left_Y-d_left_X;



powerE = d_left_Y+d_left_X;

}

else



{

powerD = d_left_Y+d_left_X;

powerE = d_left_Y-d_left_X;

}

//Arm code



//Move shoulder up

if ((int)currState.triang==0)

{

powerF = -50;



tri=(int)currState.triang;

}

//Move shoulder down



if ((int)currState.square==0)

{

powerF = 10;



squ=(int)currState.square;

}

//Move elbow up



if ((int)currState.circle==0)

{

powerG = -50;



cir=(int)currState.circle;

}

//Move elbow down



if ((int)currState.cross==0)

{

powerG = 5;



cro=(int)currState.cross;

}

//Turn off motors



if ((int)currState.cross==1 && (int)currState.circle==1 && (int)currState.square==1 && (int)currState.triang==1)

{

powerF = 0;



powerG = 0;

}





//Move wrist L/R

if ((int)currState.r_j_y<-50)

{

servo[servo1]=ServoValue[servo1]+2;



while(ServoValue[servo1] != servo[servo1])

{

a=ServoValue[servo1];



}

}
if ((int)currState.r_j_y>50)

{

servo[servo1]=ServoValue[servo1]-2;



while(ServoValue[servo1] != servo[servo1])

{

a=ServoValue[servo1];



}

}

//Move wrist U/D



if ((int)currState.r_j_x<-50)

{

servo[servo2]=ServoValue[servo2]-2;



while(ServoValue[servo2] != servo[servo2])

{

b=ServoValue[servo2];



}

}
if ((int)currState.r_j_x>50)

{

servo[servo2]=ServoValue[servo2]+2;



while(ServoValue[servo2] != servo[servo2])

{

b=ServoValue[servo2];



}

}

//Close hand



if ((int)currState.r1==0)

{

servo[servo3]=ServoValue[servo3]+3;



while(ServoValue[servo3] != servo[servo3])

{

c=ServoValue[servo3];



}

}

//Open hand



if ((int)currState.l1==0)

{

servo[servo3]=ServoValue[servo3]-3;



while(ServoValue[servo3] != servo[servo3])

{

c=ServoValue[servo3];



}

}

//Move arm right



if ((int)currState.r2==0)

{

servo[servo4]=ServoValue[servo4]+20;



while(ServoValue[servo4] != servo[servo4])

{

d=ServoValue[servo4];



}

}




//Here is the code that calls the emotional states using the

//directional buttons

//Set state to happy if up arrow is pressed on the d-pad

if ((int)currState.d==0)

{

string msg1 = "c:\\VSA\\Shrthppy\r";



MSHIDsendCommand(MSHID, MSHID_DDATA);

//MSHID_MOD_LGUI = windows key, the next argument is a key

//input 'r'

//'WINDOWS-r' opens the run command box

MSHIDsendKeyboardData(MSHID, MSHID_MOD_LGUI, 0x15);

MSHIDsendCommand(MSHID, MSHID_XMIT);

wait1Msec(1000);

MSHIDsendCommand(MSHID, MSHID_ASCII);

MSHIDsendString(MSHID, msg1);

//Wait 2 seconds to ensure no accidental double press

wait1Msec(2000);

}

//Set state to sad if down arrow is pressed on the d-pad



if ((int)currState.b==0)

{ //double slash so it will see a single one (\n problem)

// \r for return

string msg1 = "c:\\VSA\\Shortsad\r";

MSHIDsendCommand(MSHID, MSHID_DDATA);

//MSHID_MOD_LGUI = windows key, the next argument is a key

//input 'r'

//'WINDOWS-r' opens the run command box

MSHIDsendKeyboardData(MSHID, MSHID_MOD_LGUI, 0x15);

MSHIDsendCommand(MSHID, MSHID_XMIT);

wait1Msec(1000);

MSHIDsendCommand(MSHID, MSHID_ASCII);

MSHIDsendString(MSHID, msg1);

//Wait 2 seconds to ensure no accidental double press

wait1Msec(2000);

}

//Set state to long happy if left arrow is pressed on the d-pad



if ((int)currState.a==0)

{

string msg1 = "c:\\VSA\\Longhappy\r";



MSHIDsendCommand(MSHID, MSHID_DDATA);

//MSHID_MOD_LGUI = windows key, the next argument is a key

//input 'r'

//'WINDOWS-r' opens the run command box

MSHIDsendKeyboardData(MSHID, MSHID_MOD_LGUI, 0x15);

MSHIDsendCommand(MSHID, MSHID_XMIT);

wait1Msec(1000);

MSHIDsendCommand(MSHID, MSHID_ASCII);

MSHIDsendString(MSHID, msg1);

//Wait 2 seconds to ensure no accidental double press

wait1Msec(2000);

string msg2 = "c:\\VSA\\Neutral\r";

MSHIDsendCommand(MSHID, MSHID_DDATA);

//MSHID_MOD_LGUI = windows key, the next argument is a key

// input 'r'

//'WINDOWS-r' opens the run command box

MSHIDsendKeyboardData(MSHID, MSHID_MOD_LGUI, 0x15);

MSHIDsendCommand(MSHID, MSHID_XMIT);

wait1Msec(1000);

MSHIDsendCommand(MSHID, MSHID_ASCII);

MSHIDsendString(MSHID, msg2);

//Wait 2 seconds to ensure no accidental double press

wait1Msec(2000);

}




}

//Set state to long sad if left arrow is pressed on the d-pad

if ((int)currState.c==0)

{

string msg1 = "c:\\VSA\\Longsad\r";



MSHIDsendCommand(MSHID, MSHID_DDATA);

//MSHID_MOD_LGUI = windows key, the next argument is a key

//input 'r'

//'WINDOWS-r' opens the run command box

MSHIDsendKeyboardData(MSHID, MSHID_MOD_LGUI, 0x15);

MSHIDsendCommand(MSHID, MSHID_XMIT);

wait1Msec(1000);

MSHIDsendCommand(MSHID, MSHID_ASCII);

MSHIDsendString(MSHID, msg1);

//Wait 4 seconds to ensure no accidental double press

wait1Msec(4000);

string msg2 = "c:\\VSA\\Neutral\r";

MSHIDsendCommand(MSHID, MSHID_DDATA);

//MSHID_MOD_LGUI = windows key, the next argument is a key

//input 'r'

//'WINDOWS-r' opens the run command box

MSHIDsendKeyboardData(MSHID, MSHID_MOD_LGUI, 0x15);

MSHIDsendCommand(MSHID, MSHID_XMIT);

wait1Msec(1000);

MSHIDsendCommand(MSHID, MSHID_ASCII);

MSHIDsendString(MSHID, msg2);

//Wait 2 seconds to ensure no accidental double press

wait1Msec(2000);

}

/*



powerD = d_left_Y-d_left_X;

powerE = d_left_Y+d_left_X;*/

// powering motors

motor[motorD] = powerD/3;

motor[motorE] = powerE/3;

motor[motorF] = powerF;

motor[motorG] = powerG;
wait10Msec(10);
// This display is used to troubleshoot.

// Change the rev number when you chane the code to verify

// correct code loaded

nxtDisplayTextLine(1,"left X val: %d", d_left_X);

nxtDisplayTextLine(2,"left Y val: %d", d_left_Y);

nxtDisplayTextLine(3,"motorD: %d", powerD);

nxtDisplayTextLine(4,"motorE: %d", powerE);

nxtDisplayTextLine(5,"Rev2.79");


}
wait10Msec (100);
StopAllTasks ();

}




Appendix B: Tetrix Kit Contents

from http://www.legoeducation.us/sharedimages/resources/991520Elements.JPG

1   ...   11   12   13   14   15   16   17   18   ...   41


The database is protected by copyright ©dentisty.org 2016
send message

    Main page