Programmation en NXC (extraits) :
// Jambe droite : Moteur B + Capteur de contact 1 #define RIGHT_MOTOR OUT_B #define RIGHT_TOUCH S1 #define RIGHT_TOUCH_VAL SENSOR_1 // Valeur de S1 // Jambe gauche : Moteur C + Capteur de contact 2 #define LEFT_MOTOR OUT_C #define LEFT_TOUCH S2 #define LEFT_TOUCH_VAL SENSOR_2 // Valeur de S2 // Les 2 jambes à la fois #define MOTORS OUT_BC // Marche avant / arrière #define FORWARDS 1 #define BACKWARDS 2 // Les autres capteurs utilisés /////////////////////////////// // Capteur ultrasonique #define ULTRASONIC S4 #define ACTIVE_DISTANCE 30 // IMU (Inertial Motion Unit) // https://www.dexterindustries.com/manual/imu-sensor/ #define IMU S3 #define IMU_ACC_ADDR 0x3A #define IMU_GYR_ADDR 0xD2 // Les autres capteurs potentiels ///////////////////////////// // Capteur de couleurs #define COLOR S3 #define COLOR_VAL SENSOR_3 // Valeur de S3 // Capteur de sons #define SOUND S4 #define SOUND_VAL SENSOR_4 // Valeur de S4
void walkAngle (int direction, long angle) {
ResetTachoCount(MOTORS);
switch (direction) {
case FORWARDS:
// Les moteurs avancent synchronisés à la vitesse de 50 %
OnFwdReg(MOTORS, 50, OUT_REGMODE_SYNC);
while (MotorTachoCount(RIGHT_MOTOR) != angle);
break;
case BACKWARDS:
// Les moteurs reculent synchronisés à la vitesse de 50 %
OnRevReg(MOTORS, 50, OUT_REGMODE_SYNC);
while (MotorTachoCount(RIGHT_MOTOR) != -angle);
break;
}
Off(MOTORS);
}
void walkDuration (int direction, unsigned long duration) {
unsigned long t0 = CurrentTick(); // Renvoie le temps en ms
switch (direction) {
case FORWARDS:
// Les moteurs avancent synchronisés à la vitesse de 50 %
OnFwdReg(MOTORS, 50, OUT_REGMODE_SYNC);
while (CurrentTick () - t0 < duration);
break;
case BACKWARDS:
// Les moteurs reculent synchronisés à la vitesse de 50 %
OnRevReg(MOTORS, 50, OUT_REGMODE_SYNC);
while (CurrentTick () - t0 < duration);
break;
}
Off(MOTORS);
}
