int ColorJudge::isColorRGB(){
rgb_raw_t rgb_val;
ev3_color_sensor_get_rgb_raw(EV3_PORT_2, &rgb_val);
if ( (30 < rgb_val.r && rgb_val.r < 60) &&
(90< rgb_val.g && rgb_val.g <150) &&
(10< rgb_val.b && rgb_val.b < 40) ) { //green
msg_f("Green", 2);
return 3;
} else if ( (20 < rgb_val.r && rgb_val.r < 40) &&
(60 < rgb_val.g && rgb_val.g < 150) &&
(30 < rgb_val.b && rgb_val.b < 130)) { //blue
msg_f("Blue", 2);
return 2;
} else if ( (100 < rgb_val.r ) &&
(rgb_val.g < 100) &&
(10 < rgb_val.b && rgb_val.b < 25)) { //red
msg_f("Red", 2);
return 5;
}else if ( (150 < rgb_val.r && rgb_val.r < 255) &&
(200 < rgb_val.g) &&
(0 < rgb_val.b && rgb_val.b < 50) ) { //yellow
msg_f("Yellow", 2);
return 4;
} else {
return 0;
}
}
void Tracer::run() {
const float Kp = 0.83; // 比例係数
const int target = 10; // 白・黒の中間値
const int bias = 0;
msg_f("running...", 1);
int diff = colorSensor.getBrightness() - target;
float turn = Kp * diff + bias;
leftWheel.setPWM(pwm - turn);
rightWheel.setPWM(pwm + turn);
}
void Lifter::terminate() {
msg_f("Stopped.", 1);
liftHand.stop();
}
void Tracer::terminate() {
msg_f("Stopped.", 1);
leftWheel.stop();
rightWheel.stop();
}