 | Code: #pragma config(Sensor, S1, xhome, sensorTouch) #pragma config(Sensor, S2, yHome, sensorSONAR) #pragma config(Sensor, S3, zhome, sensorTouch) #pragma config(Motor, motorA, xMotor, tmotorNXT, PIDControl, reversed, encoder) #pragma config(Motor, motorB, yMotor, tmotorNXT, PIDControl, encoder) #pragma config(Motor, motorC, zMotor, tmotorNXT, PIDControl, reversed, encoder)
void zresetowaniePozycjiX(); void zresetowaniePozycjiY(); void zresetowaniePozycjiZ();
void przesunX(int docelowaX); void przesunY(int docelowaY); void przesunZ(int docelowaZ);
void wiercenie(int tablicax ,int tablicay, int tablicawiercenie);
int tablicax[5]={-51,-52,-53,-54,-55,}; int tablicay[5]={50,51,53,54,55,}; int tablicawiercenie[5]={55,56,57,58,59,};
task main() { zresetowaniePozycjiX(); wait1Msec(1); zresetowaniePozycjiY(); wait1Msec(1); zresetowaniePozycjiZ(); wait1Msec(1); //reseting positions wait1Msec(1); przesunX(-50); wait1Msec(1000); przesunY(-50); wait1Msec(1000); przesunZ(50); wait1Msec(4000); //movement to brick nMotorEncoder[motorA] = 0; nxtDisplayTextLine(1,"%d", nMotorEncoder[motorA]); nMotorEncoder[motorB] = 0; nxtDisplayTextLine(2,"%d", nMotorEncoder[motorB]); nMotorEncoder[motorC] = 0; nxtDisplayTextLine(3,"%d", nMotorEncoder[motorC]); //reset motor encoder & show void wiercenie; wait10Msec(1000);
}
void wiercenie(int tablicax, int tablicay, int tablicawiercenie) { nxtDisplayTextLine(0,"Test %d %d %d",tablicax,tablicay,tablicawiercenie); }
void zresetowaniePozycjiX() {
while(SensorValue[S1]==0) { nxtDisplayTextLine(1,"%d", nMotorEncoder[motorA]); nxtDisplayTextLine(2,"%d", SensorValue[S1]); motor[motorA] = 10; } motor[motorA] = 0; nMotorEncoder[motorA] = 0; }
void zresetowaniePozycjiY() {
while(SensorValue[S2]>5) { nxtDisplayTextLine(1,"%d", nMotorEncoder[motorB]); nxtDisplayTextLine(2,"%d", SensorValue[S2]); motor[motorB] =10; } motor[motorB] =0; nMotorEncoder[motorB] = 0; } void zresetowaniePozycjiX() {
while(SensorValue[S1]==0) { nxtDisplayTextLine(1,"%d", nMotorEncoder[motorA]); nxtDisplayTextLine(2,"%d", SensorValue[S1]); motor[motorA] = 10; } motor[motorA] = 0; nMotorEncoder[motorA] = 0; } void zresetowaniePozycjiZ() {
while(SensorValue[S3]==0) { nxtDisplayTextLine(1,"%d", nMotorEncoder[motorC]); nxtDisplayTextLine(2,"%d", SensorValue[S3]); motor[motorC] = -10; } motor[motorC] = 0; nMotorEncoder[motorC] = 0; }
void przesunX(int docelowaX) { int aktualna1=nMotorEncoder[motorA]; int roznica1=aktualna1-docelowaX;
while(aktualna1 != docelowaX) { aktualna1 = nMotorEncoder[motorA]; roznica1 = aktualna1 - docelowaX; motor[motorA] =-10; wait1Msec(-roznica1); { nxtDisplayTextLine(1,"%d", aktualna1); nxtDisplayTextLine(2,"%d", docelowaX); nxtDisplayTextLine(3,"%d", roznica1); } } motor[motorA] = 0; }
void przesunY(int docelowaY) { int aktualna = nMotorEncoder[motorB]; int roznica = aktualna - docelowaY;
while (aktualna != docelowaY) { aktualna = nMotorEncoder[motorB]; roznica = aktualna - docelowaY; motor[motorB]=-10; wait1Msec(docelowaY);
nxtDisplayTextLine(1,"%d", aktualna); nxtDisplayTextLine(2,"%d", docelowaY); nxtDisplayTextLine(3,"%d", roznica); } motor[motorB] = 0; }
void przesunZ(int docelowaZ) { int aktualna2=nMotorEncoder[motorC]; int roznica2=aktualna2-docelowaZ;
while(aktualna2 != docelowaZ) { aktualna2 = nMotorEncoder[motorC]; roznica2 = aktualna2 - docelowaZ; motor[motorC] =10; wait1Msec(--roznica2); { nxtDisplayTextLine(1,"%d", aktualna2); nxtDisplayTextLine(2,"%d", docelowaZ); nxtDisplayTextLine(3,"%d", roznica2); } } motor[motorC] = 0; } |  |