Controlling a claw based on position of potentiometer?

#1

Im trying to make the claw work by opening and closing based on the potentiometer, my code works a little bit but it is inconsistant and doesn’t work at the moment, anyone have suggestions?

#pragma config(Sensor, in2, potentiometer, sensorPotentiometer)
#pragma config(Motor, port1, clawMotor, tmotorServoContinuousRotation, openLoop, reversed)

task claw_out();
task claw_in();

task main()
{
	int selec;
	while(true)
	{
		if (SensorValue[potentiometer]>4000)
		{
			selec =1;
			waitUntil(SensorValue[potentiometer]<150);
		}
		else	if (SensorValue[potentiometer]>3950)
		{
			selec =2;
			waitUntil(SensorValue[potentiometer]<100);
		}

		switch (selec)
		{
		case 1:
			startTask(claw_in);
			stopTask(claw_out);
			break;
			/*-----------------------------*/
		case 2:
			startTask(claw_out);
			stopTask(claw_in);
			break;
			/*-----------------------------*/
		default:
			stopTask(claw_in);
			stopTask(claw_out);
			break;
		}
	}
}

task claw_out(){
	waitUntil (SensorValue[potentiometer]>100);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>200);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>300);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>400);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>500);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>600);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>700);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>800);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>900);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>1000);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>1100);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>1200);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>1300);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>1400);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>1500);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>1600);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>1700);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>1800);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>1900);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>2000);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>2100);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>2200);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>2300);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>2400);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>2500);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>2600);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>2700);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>2800);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>2900);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>3000);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>3100);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>3200);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>3300);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>3400);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>3500);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>3600);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>3700);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>3800);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>3900);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
	waitUntil (SensorValue[potentiometer]>4000);{
		startMotor (clawMotor, 63.5);
		wait (.02);
		stopMotor (clawMotor);
	}
}
task claw_in(){
	{
		waitUntil (SensorValue[potentiometer]<4000);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<3900);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<3800);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<3700);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<3600);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<3500);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<3400);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<3300);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<3200);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<3100);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<3000);{
			startMotor (clawMotor,-25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<2900);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<2800);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<2700);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<2600);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<2500);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<2400);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<2300);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<2200);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<2100);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<2000);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<1900);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<1800);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<1700);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<1600);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<1500);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<1400);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<1300);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<1200);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<1100);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<1000);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<900);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<800);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<700);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<600);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<500);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<400);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
		waitUntil (SensorValue[potentiometer]<300);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}

		waitUntil (SensorValue[potentiometer]<200);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);

		}
		waitUntil (SensorValue[potentiometer]<100);{
			startMotor (clawMotor, -25);
			wait (.02);
			stopMotor (clawMotor);
		}
	}
}
0 Likes
#2

One of the problems with the if(sensor<target){left} if else(sensor>target){right} else{stop} is its not very good at getting to its target at desirable speeds with accuracy. This is where proportional loops come in. Speed is what speed your motor will go at. Error is the difference between your target between your current position. kP is your proportional value.

error = target - sensorValue;
speed = error * kP;

You will notice with the error it gets smaller the closer the current claw position gets closer to where you want it. And the further away it is the larger it is. This lets your motor go full speed when far away from your target, but it will slow down as it gets closer to prevent overshooting the target.

What you might realize is that the error probably doesn’t match up with how fast you want your motor to go. This is where kP comes in. kP is your proportional (hey that’s the name of the show), and it is a value that multiplies your error to a value your motors will be happy with.

The best way to tune kP easily (IMO) is to start it at 1 and test your setpoints. If its too slow, increase the size of kP. If it starts oscillating back and forth, lower it. Rinse and repeat till you found a value you like. If a P-Loop doesn’t satisfy you. There is a more complicated version called a PID-Loop (Check the first wikipedia link).

Good luck. Also if I may inquire why do you constantly turn on and off your motor? And with that why you repeat the same code over and over but at different positions?

1 Like