arm and gripper programming guide ( · pdf file124 arm and gripper programming guide...

124

Arm and Gripper Programming Guide (ROBOTC®)

Introduction:

Inthisguide,theRangerBotwillbeprogrammedtofollowaline,pickupanobject,andputitinabox.ThisguideisforusewiththeROBOTC®programminglanguage.

Getting Started:

1.Tostarttheprogram,typetheMain Taskfunction,followedbyanopening brace.

taskmain()

{

Defining Variables:

2.Declarefiveinteger variablesasseeninthecodebelow.Besideeachvariablebelow,therearetwoslashes(//)followedbywhatthatvariableisrepresenting.Thesemarkacomment.NoticethatcommentsappearingreenwhencodeistypedintoROBOTC.

Note:AcommentisnotpartofthecodethattheNXTbrickwillexecute,butithelpstheprogrammerbyallowingnotestobeaddedtodescribewhathasbeendone.

intnValue; //lightvalue

intmValue; //sonarvalue

intaPosition; //servoarmposition

intgPosition; //gripperservoposition

intmSpeed; //motorspeed

3.TypetheWaitfunctionandsetittowait 50 milliseconds toallowtherobottoinitializemotorsandsensors.

wait1Msec(50);

4.Typetheservo positioncommandtoinitializethepositionofbothservos.Thismustbesettoavaluebetween0and255,representingapositionbetween0°and180°.Thisistheservo’sfullrangeofmotion.

servo[servo1]=200;

servo[servo2]=100;

5.TypetheWait functionandsetittowait1secondwhiletheservomotorsgetintoposition.

wait1Msec(1000);

6.Addaninfinite While Looptomakethecontainingcodeexecuteforever.

while(true)

{

ExtensionsTETRIX®GettingStartedGuide

125

7.Setthedefinedvariablestoavalueanddisplaythemonthescreen.

a.SetthenValuevariabletothevalueofthelightsensorandthemValuevariabletothevalueoftheultrasonicsensor.

b.UsetheNXT write to screencommandtoputthemonthescreen.

Note:Thiscommandrequiresthreepiecesofinformation(calledparameters)inordertowritebothtextandavariabletothescreen:

i.Thelinenumber

ii.Thetext(inquotations)

iii.Thetext(inquotations)whichincludesadisplayvariablecaller.

Note:Inthiscase,%disthedisplayvariablecaller.Intheplaceof%d,anintegervariableisdisplayedonthescreen.

Thisinformationisinsidethebracketsfollowingthecommand,separatedbycommas,asseenbelow.

nValue=SensorValue[LightSensor];

nxtDisplayTextLine(0,"LightValue:%d",nValue);

mValue=SensorValue[SonarSensor];

nxtDisplayTextLine(2,"SonarValue:%d",mValue);

8.Belowthetextnowwrittenonthescreen,displayamessagethatsays“Searching For Target."Becauseavariablewillnotbedisplayed,onlytwoparameterswillneedtobesenttothefunction:thelinenumberandthetext.

nxtDisplayCenteredTextLine(4,"Searching");

nxtDisplayCenteredTextLine(5,"ForTarget");

9.Thenextfewlinesofcodewilldecreasethemotorspeedasthetargetisbeingapproached.Addaconditional statementsothatiftheultrasonic sensor valueisgreaterthan30,themSpeed variablewillbesetto30,butifitisequaltoorlessthan30,themSpeed variable will be set to 15.

if(SensorValue[SonarSensor]>30)

{

mSpeed=30;

}

else

{

mSpeed=15;

}


TETRIX®GettingStartedGuideExtensions

126

Line Follower Code:

10.Addtheline-followingcodethatwascreatedinLineFollowerExtensionPart3.

if(SensorValue[LightSensor]<50)

{

motor[motorD]=mSpeed;

motor[motorE]=0;

}

else

{

motor[motorE]=mSpeed;

motor[motorD]=0;

}

Grab an Object:

11.Addaconditional statementtocheckiftherobotiswithin17cmoftheobjectitispickingup.Iftherobotiswithin17cmoftheobject,display“Found! Grabbing..”ontherobotscreenandstopthemotors.

if(SensorValue[SonarSensor]<17)

{

nxtDisplayCenteredTextLine(4,“Found!”);

nxtDisplayCenteredTextLine(5,“Grabbing..”);

motor[motorE]=0;

motor[motorD]=0;

12.Adda Wait functiontopauseforonesecondtogivethemotorstimetostopbeforethenextactionisexecuted.

wait1Msec(1000);

13.AddaFor Loop whichmakesthearmservomovefromitscurrentpositionat100toalowerpositionof190,waiting10milliseconds betweeneachmovement.

for(aPosition=100;aPosition<190;aPosition++)

{

servo[servo2]=aPosition;

wait10Msec(1);

}

14.TypetheWait functiontomaketheprogrampausefor0.5 seconds.

wait1Msec(500);



127

15.Addanother For Loop tomakethegripperservomovefromitscurrentpositionat180toapositionof80,waiting10millisecondsbetweeneachmovementinordertoclosethegripper.

for(gPosition=180;gPosition>80;gPosition--)

{

servo[servo1]=gPosition;

wait10Msec(1);

}

16.Typethe Wait functiontomaketheprogrampausefor0.5seconds.

wait1Msec(500);

17.AddaFor Looptomakethearmservomovefromitscurrentpositionat160toahigherpositionof25,waiting10millisecondsbetweeneachmovement.

for(aPosition=160;aPosition>25;aPosition--)

{

servo[servo2]=aPosition;

wait10Msec(1);

}

18.Addaconditional statementtocheckifthevalueofthetouchsensoris1(orpressed).Ifitis,stopbothmotorsanddisplaythelightandultrasonicsensorvaluesonthedisplay.Also,displaytextthatsays“Status: Got it!,”playasound(called“soundBlip”),andwaitsonesecondforthemotorstostopcompletely.

if(SensorValue[Touch]==1)

{

motor[motorE]=0;

motor[motorD]=0;





nxtDisplayCenteredTextLine(4,"Status:");

nxtDisplayCenteredTextLine(5,"GotIt!");

PlaySound(soundBlip);

wait1Msec(1000);

}

19.Addanotherconditional statement tocheckifthevalueofthetouchsensoris0(orunpressed).Ifitis,stopbothmotorsanddisplaythelightandultrasonicsensorvaluestothedisplay.



128

20.Also,displaytextthatsays“Status: Missed It!”andwaits0.5secondsforthemotorstostopcompletely.

if(SensorValue[Touch]==0)

{

motor[motorE]=0;

motor[motorD]=0;





nxtDisplayCenteredTextLine(4,"Status:");

nxtDisplayCenteredTextLine(5,"MissedIt!");

wait1Msec(500);

}

End the Program:

21.Typeaclosing brace toendtheconditionalstatementinStep11.

}

22.Typeaclosing bracetoendtheinfiniteWhileLoopfromStep6.

}

23.Typeaclosing bracetoendtheprogram.

}

Completed Code:

taskmain()

{

intnValue;//lightvalue

intmValue;//sonarvalue

intaPosition;//servoarmposition

intgPosition;//gripperservoposition

intmSpeed;//MotorSpeed

wait1Msec(50);//Theprogramwaits50millisecondstoinitializethelightsensor.

servo[servo1]=200;//Initialstartupgripperservoposition

servo[servo2]=80;//Initialstartuparmservoposition



129

mSpeed=30;

wait1Msec(1000);//waitforarmandgrippertogettoposition

while(true)//Infiniteloop

{





nxtDisplayCenteredTextLine(4,"Searching");

nxtDisplayCenteredTextLine(5,"ForTarget");

///Decreasespeedasweapproachtarget

if(SensorValue[SonarSensor]>30)

{

mSpeed=30;

}

else

{

mSpeed=15;

}

////LinefollowingHere;

if(SensorValue[LightSensor]<50)//IftheLightSensorreadsavaluelessthan50:

{

motor[motorD]=mSpeed;//MotorDisrunata30powerlevel.

motor[motorE]=0;//MotorEisrunata0powerlevel.

}

else//IftheLightSensorreadsavaluegreaterthanorequalto50:

{

motor[motorE]=mSpeed;//MotorEisrunata30powerlevel.

motor[motorD]=0;//MotorDisrunata0powerlevel.

}

////Sonarranginghere;Stopattargetdistance,positiongripperandarm

if(SensorValue[SonarSensor]<17)//Targetsensedistance(centimeters)


Completed Code (continued):


130


{

nxtDisplayCenteredTextLine(4,"Found!");//updatedisplay

nxtDisplayCenteredTextLine(5,"Grabbing..");//updatedisplay

motor[motorE]=0;//MotorBisoff

motor[motorD]=0;//MotorCisoff

wait1Msec(1000);//Pausefor1second

for(aPosition=100;aPosition<190;aPosition++)

{

servo[servo2]=aPosition;//lowerarm

wait10Msec(1);

}

wait1Msec(500);//Pausefor1/2second

for(gPosition=180;gPosition>80;gPosition--)

{

servo[servo1]=gPosition;//closegripper

wait10Msec(1);

}

wait1Msec(500);//Pausefor1/2second

for(aPosition=160;aPosition>25;aPosition--)

{

servo[servo2]=aPosition;//raisearm

wait10Msec(1);

}

if(SensorValue[Touch]==1)//checkgrippersensorstatus

{




nxtDisplayTextLine(0,"LightValue:%d",nValue);//updatedisplay


nxtDisplayTextLine(2,"SonarValue:%d",mValue);//updatedisplay

nxtDisplayCenteredTextLine(4,"Status:");//updatedisplay

nxtDisplayCenteredTextLine(5,"GotIt!");//updatedisplay

PlaySound(soundBlip);//Playthesound,'soundBeepBeep'.

wait1Msec(1000);



131

}

if(SensorValue[Touch]==0)//checkgrippersensorstatus

{




nxtDisplayTextLine(0,"LightValue:%d",nValue);//updatedisplay


nxtDisplayTextLine(2,"SonarValue:%d",mValue);//updatedisplay

nxtDisplayCenteredTextLine(4,"Status:");//updatedisplay

nxtDisplayCenteredTextLine(5,"MissedIt!");//updatedisplay

wait1Msec(500);

}

}

}




arm and gripper programming guide ( · pdf file124 arm and gripper programming guide...

Documents