 | Code: #pragma config(Sensor, S1, Touch1, sensorTouch) #pragma config(Sensor, S2, Touch2, sensorTouch) #pragma config(Sensor, S3, Touch3, sensorTouch) #pragma config(Motor, motorA, Motor1, tmotorNormal, PIDControl, encoder) #pragma config(Motor, motorB, Motor2, tmotorNormal, PIDControl, encoder) #pragma config(Motor, motorC, Motor3, tmotorNormal, PIDControl, encoder) //*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(NXT)
int valvePos[4]; // array to store the current valve positions int leverLFT = 50; // motor left power int leverRGT = -50; // motor right power
// move the motor until it is stopped. void valveOn(short MotorX, int Direction) { int motorBack = 40; // motor relief int mpos; int menc; int mpow;
// rotate the motor until it has stopped (stays at the same value for more than 25msec) while(mpos != menc) { mpos = nMotorEncoder[MotorX]; motor[MotorX] = Direction; wait1Msec(25); // increase this delay slightly if the valve doesn't move menc = nMotorEncoder[MotorX]; } motor[MotorX] = 0; //wait1Msec(10); // back the lever of a little to stop the motor from squealing. if (Direction == leverRGT) { // right mpos = nMotorEncoder[MotorX] + motorBack; mpow = leverLFT; } else { //left mpos = nMotorEncoder[MotorX] - motorBack; mpow = leverRGT; } nMotorEncoderTarget[MotorX] = mpos; while (nMotorEncoder[MotorX] != mpos) { motor[MotorX] = mpow; } motor[MotorX] = 0; return; }
void valveMove(short MotorX, int Valve, int Position) { // valve positions: // 4 unkown (on startup) // 1 lever right motor: -50 // 2 lever left motor: +50
// if the valve state is unknown (4), set it up if (valvePos[Valve] == 4) { valveOn(MotorX,leverRGT); valvePos[Valve] = 1; return; }
// if current position is the same as the new position, don't bother if (valvePos[Valve] == Position) { return; }
switch (Position) { case 1: // 1 lever right valveOn(MotorX,leverRGT); break; case 2: // 2 lever left valveOn(MotorX,leverLFT); break; } valvePos[Valve] = Position; return; }
task main() {
// setup motor stuff nPowerDownDelayMinutes = 240; // 4 hours bNoPowerDownOnACAdaptor = true; nMotorPIDSpeedCtrl[Motor1] = mtrSpeedReg; nMotorPIDSpeedCtrl[Motor2] = mtrSpeedReg; nMotorPIDSpeedCtrl[Motor3] = mtrSpeedReg; nMaxRegulatedSpeedNxt = 750;
nMotorEncoder[Motor1] = 0; nMotorEncoder[Motor2] = 0; nMotorEncoder[Motor3] = 0;
// valve positions are unkown at startup valvePos[1] = 4; valvePos[2] = 4; valvePos[3] = 4; //set valves to start positions valveMove(Motor1,1,2); // valveMove(motor, valvePos[no], position) valveMove(Motor2,2,1); valveMove(Motor3,3,1);
wait1Msec(1000); PlaySound(soundBeepBeep);
// main loop, move the relivant valve when the touch sensor is pressed. while (nNxtButtonPressed != 3) { nxtDisplayBigStringAt (0,60,"M1: %d ", valvePos[1]); nxtDisplayBigStringAt (0,40,"M2: %d ", valvePos[2]); nxtDisplayBigStringAt (0,20,"M3: %d ", valvePos[3]);
if (SensorValue(Touch1) == 1) { if (valvePos[1] == 1) { valveMove(Motor1,1,2); wait1Msec(100); } else { valveMove(Motor1,1,1); wait1Msec(100); } } if (SensorValue(Touch2) == 1) { if (valvePos[2] == 1) { valveMove(Motor2,2,2); wait1Msec(100); } else { valveMove(Motor2,2,1); wait1Msec(100); } } if (SensorValue(Touch3) == 1) { if (valvePos[3] == 1) { valveMove(Motor3,3,2); wait1Msec(100); } else { valveMove(Motor3,3,1); wait1Msec(100); } } wait1Msec(200); } } |  |