My Program
#pragma config(Sensor, S1, touch, sensorTouch)
#pragma config(Sensor, S2, sound, sensorSoundDB)
#pragma config(Sensor, S3, light, sensorLightActive)
#pragma config(Sensor, S4, sonar, sensorSONAR)
#pragma config(Motor, motorA, clawMotor, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, motorB, rightMotor, tmotorNXT, PIDControl, driveRight, encoder)
#pragma config(Motor, motorC, leftMotor, tmotorNXT, PIDControl, driveLeft, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
const float kp = 0.57;
const float ki = 0.00000009;
const float kd = 1;
const int targetValue = 68;
int reading;
float turn;
float error=0;
float last_error=0;
float integral=0;
float derivative(float error, float last_error);
const float Botwidth = 17;//cm
const float WheelDia = 5.6;//cm
const float WheelCircum = WheelDia * PI;
const float SpinCircum = Botwidth*PI;
float dist();
float spinDegToTicks(int spinDeg);
int distToTicks(float dist);
void forward(float speed);
void forwardDist(float dist, float speed);
void spin(int spinDeg);
void openClaw();
void closeClaw();
void backward();
void spinLeft(int spinDeg);
void lineTrace(float turn);
void forwardWait(int speed);
void forward(float speed){
motor[leftMotor]=10*speed;
motor[rightMotor]=10*speed;
}
float integraladd(float error){
integral= (integral + error)/100;
return integral;
}
float derivative(float error, float last_error){
return error - last_error;
}
float correction(float error, float last_error){
return error*kp+integraladd(error)*ki+derivative(error,last_error)*kd;
}
void backward(){
motor[leftMotor]=-30;
motor[rightMotor]=-30;
}
void spin(int spinDeg){
nMotorEncoder[leftMotor]=0;
while(nMotorEncoder[leftMotor]<spinDegToTicks(spinDeg)){
motor[leftMotor]=20;
motor[rightMotor]=-20;
}
}
void spinLeft(int spinDeg){
nMotorEncoder[rightMotor]=0;
while(nMotorEncoder[rightMotor]<spinDegToTicks(spinDeg)){
motor[leftMotor]=-20;
motor[rightMotor]=20;
}
}
float spinDegToTicks(int spinDeg){
return Botwidth * PI/WheelCircum * 360/(360/spinDeg)
}
int distToTicks(float dist){
return dist/WheelCircum * 360;
}
void forwardDist(float dist, float speed){
nMotorEncoder[leftMotor]=0;
while(nMotorEncoder[leftMotor]<distToTicks(dist)){
forward(speed);
}
}
void closeClaw(){
nMotorEncoder[clawMotor]=0;
while(nMotorEncoder[clawMotor]<30){
motor[clawMotor]=50;
}
}
void openClaw(){
nMotorEncoder[clawMotor]=0;
while(nMotorEncoder[clawMotor]>-30){
motor[clawMotor]=-50;
}
}
void forwardWait(int speed){
motor[leftMotor]=10 * speed;
motor[rightMotor]=10 * speed;
delay(2000);
}
void lineTrace(float turn){
error=targetValue-SensorValue[light];
reading=SensorValue[light];
turn=correction(error, last_error);
motor[leftMotor]=26+turn;
motor[rightMotor]=26-turn;
last_error=error;
}
void clearObject(){
spin(60);
forwardDist(20, 2);
spinLeft(60);
forwardDist(50, 2);
spinLeft(60);
while(SensorValue[light]>60){
forward(2);
}
forwardDist(8, 2)
spin(60);
}
task main()
{
forward(2);
delay(2000);
while(SensorValue[sonar]>25){
while(SensorValue[light]>25){
while(SensorValue[sonar]>25){
lineTrace(turn);
}
clearObject();
}
}
}