we want the robot to avoid obstruction as well; but we don't know how; we tried possible codes but it won't satisfy our requirements. what will i do??? can anybody help me??? i'm begging.. we're on the point of giving up;
..here's our code:
Code: Select all
#include "NXCDefs.h"
#include "NBCCommon.h"
#define MOTOR_LEFT OUT_B
#define MOTOR_RIGHT OUT_C
#define MOTOR_BOTH OUT_BC
#define MOTOR_FORK OUT_A
#define COLORSENSOR SENSOR_2
#define TOUCH SENSOR_3
#define ULSONIC SensorUS(IN_1)
#define LIGHT_BLACK 115
#define LIGHT_WHITE 275
struct RGB {
unsigned int red;
unsigned int green;
unsigned int blue;
};
RGB get_color(int sensorPort) {
RGB color;
color.red = ColorSensorValue(sensorPort, INPUT_RED);
color.green = ColorSensorValue(sensorPort, INPUT_GREEN);
color.blue = ColorSensorValue(sensorPort, INPUT_BLUE);
return color;
}
unsigned int get_light(int sensorPort) {
RGB color = get_color(sensorPort);
float colorSum = color.red + color.green + color.blue;
unsigned int light = colorSum / 3.0;
return light;
}
sub follow_line() {
float kP = 1.0; //6.0
float kI = 0.0; //0.0001
float kD = 0.0; //400
float dT = 25;
// Initialize the variables:
int offset = LIGHT_WHITE - LIGHT_BLACK;
int tP = 50;
int proportional = 0;
int integral = 0;
int derivative = 0;
int lastError = 0;
while (2) {
int lightValue = get_light(IN_2);
int error = lightValue - offset;
proportional = kP * error;
integral = integral + error;
derivative = (error - lastError) * dT;
int output = proportional + kI * dT * integral + kD * derivative;
int speedLeft = tP - output;
int speedRight = tP + output;
int value = 0;
if (speedLeft > 100) {
speedLeft = 100;
} else if (speedLeft < -100) {
speedLeft = -100;
}
if (speedRight > 100) {
speedRight = 100;
} else if (speedRight < -100) {
speedRight = -100;
}
lastError = error;
while(COLORSENSOR == INPUT_BLUECOLOR){
Off(MOTOR_BOTH);
while (ULSONIC == 32){
PlayTone(330,400);
OnFwdSync(OUT_BC, 50, 0); Wait(1000);
}
while (COLORSENSOR == INPUT_GREENCOLOR) {
value = 1;
Off(MOTOR_BOTH); Wait(100);
while (ULSONIC == 24) {
PlayTone(330, 400);
OnRev(MOTOR_FORK, 100); Wait(2500);
RotateMotor(MOTOR_BOTH, 50, 400);
Off(MOTOR_BOTH);
OnFwd(MOTOR_FORK, 100); Wait(2500);
while (TOUCH == 1) {
OnFwd(MOTOR_LEFT, 80);
OnRev(MOTOR_RIGHT,80);
Wait(999);
break;
}
}
}
while (COLORSENSOR == INPUT_REDCOLOR) {
Off(MOTOR_BOTH); Wait(100);
while(ULSONIC == 24) {
PlayTone(330, 400);
OnRev(MOTOR_FORK, 100); Wait(2500);
RotateMotor(MOTOR_BOTH, 50, 400);
Off(MOTOR_BOTH);
OnFwd(MOTOR_FORK, 100); Wait(2500);
while(TOUCH) {
OnFwd(MOTOR_LEFT, 80);
OnRev(MOTOR_RIGHT,80);
Wait(900);
break;
}
while(TOUCH == 0) {
Off(MOTOR_BOTH);
}
}
}
while (COLORSENSOR == INPUT_YELLOWCOLOR) {
Off(MOTOR_BOTH); Wait(100);
while(ULSONIC == 24) {
PlayTone(330, 400);
OnRev(MOTOR_FORK, 100); Wait(2500);
RotateMotor(MOTOR_BOTH, 50, 400);
Off(MOTOR_BOTH);
OnFwd(MOTOR_FORK, 100); Wait(2500);
while(TOUCH) {
OnFwd(MOTOR_LEFT, 80);
OnRev(MOTOR_RIGHT,80);
Wait(900);
break;
}
while (TOUCH == 0){
Off(MOTOR_BOTH);
}
}
}
}
OnFwd(MOTOR_LEFT, speedLeft);
OnFwd(MOTOR_RIGHT, speedRight);
}
}
void setup() {
SetSensorColorFull(IN_2);
SetSensorLowspeed(IN_1);
SetSensorTouch(IN_3);
}
void grab(){
OnFwd(MOTOR_FORK, 100);
Wait(1000);
Off(OUT_A);
}
sub not_touch(){
while(!TOUCH){
Off(MOTOR_BOTH);
}
}
task main() {
grab();
setup();
not_touch();
follow_line();
}
we also want the robot to stop when it fails to grab the object;(meaning the touch sensor is not touched)
the design of the robot is similar to the "Snatcher Robot" however we relocate the color sensor facing on the surface for the line following purposes while the touch sensor is placed in the former position of the color sensor;
