Here is the beginning of the code given by my teacher :
Code: Select all
#define LIGHT_SENSOR_PIN A0
#define BLACK_THRESHOLD 100 // Adjust this value based on the sensor characteristics
#define PROX_SENSOR_L_PIN A1
#define PROX_SENSOR_R_PIN A2
#define PROX_SENSOR_FL_PIN A3
#define PROX_SENSOR_FR_PIN A4
#define PROX_SENSOR_RL_PIN 6
#define PROX_SENSOR_RR_PIN 12
#define PROX_SENSOR_DL_PIN A5
#define PROX_SENSOR_DR_PIN 9
#define STOP_TIME 200
#define MOTOR_RF_PIN 2
#define MOTOR_RB_PIN 4
#define MOTOR_R_SPEED 3
#define MOTOR_LF_PIN 7
#define MOTOR_LB_PIN 8
#define MOTOR_L_SPEED 5
void hardware_setup() {
new DCMotor_Hbridge(MOTOR_RF_PIN, MOTOR_RB_PIN, MOTOR_R_SPEED, "ePuck_rightJoint", 2.5, 3 * 3.14159, 1);
new DCMotor_Hbridge(MOTOR_LF_PIN, MOTOR_LB_PIN, MOTOR_L_SPEED, "ePuck_leftJoint", 2.5, 3 * 3.14159, 1);
new VisionSensor(LIGHT_SENSOR_PIN, "ePuck_lightSensor", 0.1);
new ProximitySensor(PROX_SENSOR_FL_PIN, "ePuck_proxSensor3", 0.1, 1);
new ProximitySensor(PROX_SENSOR_FR_PIN, "ePuck_proxSensor4", 0.1, 1);
new ProximitySensor(PROX_SENSOR_L_PIN, "ePuck_proxSensor1", 0.1, 1);
new ProximitySensor(PROX_SENSOR_R_PIN, "ePuck_proxSensor6", 0.1, 1);
new ProximitySensor(PROX_SENSOR_RL_PIN, "ePuck_proxSensor7", 0.1, 1);
new ProximitySensor(PROX_SENSOR_RR_PIN, "ePuck_proxSensor8", 0.1, 1);
new ProximitySensor(PROX_SENSOR_DL_PIN, "ePuck_proxSensor2", 0.1, 1);
new ProximitySensor(PROX_SENSOR_DR_PIN, "ePuck_proxSensor5", 0.1, 1);
}
Code: Select all
#define LIGHT_SENSOR_PIN A0
#define BLACK_THRESHOLD 100 // Adjust this value based on your sensor characteristics
#define PROX_SENSOR_L_PIN A1
#define PROX_SENSOR_R_PIN A2
#define PROX_SENSOR_FL_PIN A3
#define PROX_SENSOR_FR_PIN A4
#define PROX_SENSOR_RL_PIN 6
#define PROX_SENSOR_RR_PIN 12
#define PROX_SENSOR_DL_PIN A5
#define PROX_SENSOR_DR_PIN 9
#define STOP_TIME 200
#define MOTOR_RF_PIN 2
#define MOTOR_RB_PIN 4
#define MOTOR_R_SPEED 3
#define MOTOR_LF_PIN 7
#define MOTOR_LB_PIN 8
#define MOTOR_L_SPEED 5
bool lightSensorActive = true;
unsigned long blackDetectionStartTime = 0;
void hardware_setup() {
new DCMotor_Hbridge(MOTOR_RF_PIN, MOTOR_RB_PIN, MOTOR_R_SPEED, "ePuck_rightJoint", 2.5, 3 * 3.14159, 1);
new DCMotor_Hbridge(MOTOR_LF_PIN, MOTOR_LB_PIN, MOTOR_L_SPEED, "ePuck_leftJoint", 2.5, 3 * 3.14159, 1);
new VisionSensor(LIGHT_SENSOR_PIN, "ePuck_lightSensor", 0.1);
new ProximitySensor(PROX_SENSOR_FL_PIN, "ePuck_proxSensor3", 0.1, 1);
new ProximitySensor(PROX_SENSOR_FR_PIN, "ePuck_proxSensor4", 0.1, 1);
new ProximitySensor(PROX_SENSOR_L_PIN, "ePuck_proxSensor1", 0.1, 1);
new ProximitySensor(PROX_SENSOR_R_PIN, "ePuck_proxSensor6", 0.1, 1);
new ProximitySensor(PROX_SENSOR_RL_PIN, "ePuck_proxSensor7", 0.1, 1);
new ProximitySensor(PROX_SENSOR_RR_PIN, "ePuck_proxSensor8", 0.1, 1);
new ProximitySensor(PROX_SENSOR_DL_PIN, "ePuck_proxSensor2", 0.1, 1);
new ProximitySensor(PROX_SENSOR_DR_PIN, "ePuck_proxSensor5", 0.1, 1);
}
void setup() {
Serial.begin(4800);
pinMode(MOTOR_RF_PIN, OUTPUT);
pinMode(MOTOR_RB_PIN, OUTPUT);
pinMode(MOTOR_R_SPEED, OUTPUT);
pinMode(MOTOR_LF_PIN, OUTPUT);
pinMode(MOTOR_LB_PIN, OUTPUT);
pinMode(MOTOR_L_SPEED, OUTPUT);
// Set speed to max
analogWrite(MOTOR_R_SPEED, 255);
analogWrite(MOTOR_L_SPEED, 255);
}
void stopRobot() {
digitalWrite(MOTOR_RF_PIN, LOW);
digitalWrite(MOTOR_RB_PIN, LOW);
digitalWrite(MOTOR_LF_PIN, LOW);
digitalWrite(MOTOR_LB_PIN, LOW);
delay(3000);
}
void avancer(){
digitalWrite(MOTOR_RF_PIN, HIGH);
digitalWrite(MOTOR_RB_PIN, LOW);
digitalWrite(MOTOR_LF_PIN, HIGH);
digitalWrite(MOTOR_LB_PIN, LOW);
delay(20);
}
void Tdroite(){
analogWrite(MOTOR_RF_PIN, 20);
analogWrite(MOTOR_RB_PIN, 150);
analogWrite(MOTOR_LF_PIN, 150);
analogWrite(MOTOR_LB_PIN, 0);
delay(20);
}
void Tgauche(){
analogWrite(MOTOR_RF_PIN, 150);
analogWrite(MOTOR_RB_PIN, 0);
analogWrite(MOTOR_LF_PIN, 20);
analogWrite(MOTOR_LB_PIN, 150);
delay(20);
}
void gauche(){
digitalWrite(MOTOR_RF_PIN, HIGH);
digitalWrite(MOTOR_RB_PIN, LOW);
digitalWrite(MOTOR_LF_PIN, LOW);
digitalWrite(MOTOR_LB_PIN, HIGH);
delay(20);
}
void droite(){
digitalWrite(MOTOR_RF_PIN, LOW);
digitalWrite(MOTOR_RB_PIN, HIGH);
digitalWrite(MOTOR_LF_PIN, HIGH);
digitalWrite(MOTOR_LB_PIN, LOW);
delay(20);
}
void checkBlackCase() {
int lightSensorValue = analogRead(LIGHT_SENSOR_PIN);
if (lightSensorActive == true) {
if (lightSensorValue < BLACK_THRESHOLD) {
// Robot is on a black case
if (blackDetectionStartTime == 0) {
// Start the timer
blackDetectionStartTime = millis();
} else {
// Check if black has been detected for the specified time
if (millis() - blackDetectionStartTime >= STOP_TIME) {
stopRobot();
lightSensorActive = false;
}
}
} else {
// Reset the timer if not on a black case
blackDetectionStartTime = 0;
}
}
}
void loop() {
int captFL = digitalRead(PROX_SENSOR_FL_PIN);
int captFR = digitalRead(PROX_SENSOR_FR_PIN);
int captL = digitalRead(PROX_SENSOR_L_PIN);
int captR = digitalRead(PROX_SENSOR_R_PIN);
int captRL = digitalRead(PROX_SENSOR_RL_PIN);
int captRR = digitalRead(PROX_SENSOR_RR_PIN);
int captDL = digitalRead(PROX_SENSOR_DL_PIN);
int captDR = digitalRead(PROX_SENSOR_DR_PIN);
int captNOIR = digitalRead(LIGHT_SENSOR_PIN);
if( captFL == 0 && captFR == 0){
avancer();
}
if( captFL == 1 || captDL == 1 || captL == 1 && captDR == 0 || captR == 0 || captFR == 0 ){
Tdroite();
}
if( captFL == 0 || captDL == 0 || captL == 0 && captDR == 1 || captR == 1 || captFR == 1 ){
Tgauche();
}
if (captFR == 1){
gauche();
}
if ( captFL == 1 ){
droite();
}
checkBlackCase();
}
}
thanks :slight_smile: