¶ Les principales problématiques
bla bla...
¶ Energie
Alimentation
¶ La distribution d'énergie
¶ Mobilité
Il faut fournir 2 plateformes issues du matériel disponible dans les armoires du labo SI.
Il faut aussi mutualiser le code Arduino.
La bibliothèque AFmotor permet de gérer la MotorShield V1 AFmotor.h adafruit_motor_shield_library-1.0.1.zip
¶ Les 2 plateformes motorisées
Une plateforme à 4 roues standards et une plateforme à 4 rous suédoises (mecanum).
¶ Télécommande PS2
Utilisation d'un module PS2X
¶ La caméra IA Luskylens
bla bla
¶ Hardware
- 1 Arduino méga
- 1 Shield Motor V1
#include <AFMotor.h> [Adafruit Motor Shield library@1.0.1] - 4 MCC 1/48
M1: Ar_Gauche, M2: Ar_Droit, M3: Av_Droit, M4: Av_Gauche - 1 Télécommande PS2X
#include <PS2X_lib.h> [PS2X_lib-master] 2.0.6
30: DAT, 32:COM, 34:ATT, 36:CLK - 1 Caméra IA Luskylens I2C
#include "HUSKYLENS.h" [HUSKYLENS@1.0.1]
#include "Wire.h" [Wire@1.0]
20: SDA, 21: SCL - 1 Block batterie 2 x 3,7V Lithium ion
- 1 pince mécanique
- 2 servo-moteurs
#include "servo.h"
¶ Software
Une structure logicielle permettant l'intégration rapide des codes des différentes équipes. Mais interdisant l'utilisation de la fonction Delay() ainsi que des fonctions bloquantes.
#define __MOTION_STOP 0
#define __MOTION_FORWARD 1
#define __MOTION_BACKWARD 2
#define __MOTION_GOLEFT 3
#define __MOTION_GORIGHT 4
bool MOTION_TURBO = false;
bool MOTION_MECANUM = false;
int MotionMove = __MOTION_STOP;
int MotionPadX = 0;
int MotionPadY = 0;
#include "remote.h"
#include "motion.h"
#include "camera.h"
#include "nippers.h"
bool RemoteModeAuto = false;
int CameraDetection = -1;
bool Searching = true;
bool CarryBack = false;
void setup() {
Serial.begin(115200);
setupRemote();
setupMotion();
setupCamera();
setupNippers();
}
void loop() {
loopRemote();
loopMotion();
loopCamera();
loopNippers();
}
#ifndef __ROBOTCLEANER_REMOTE_H__
#define __ROBOTCLEANER_REMOTE_H__
#define __REMOTE_DEBUG_ false
#include <Arduino.h>
#include <PS2X_lib.h>
PS2X ps2x;
const byte dataPin = 30;
const byte commandPin = 32;
const byte attentionPin = 34;
const byte clockPin = 36;
byte type = 0;
int error = 0;
byte vibrate = 0;
void setupRemote() {
error = ps2x.config_gamepad(clockPin, commandPin, attentionPin, dataPin, true, true); // gardez true, true à la fin même si pas de moteurs
switch(error) {
case 0:
Serial.println("Found Controller, configured successful");
Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
Serial.println("holding L1 or R1 will print out the analog stick values.");
break;
case 1:
Serial.println("No controller found, check wiring...");
break;
case 2:
Serial.println("Controller found but not accepting commands...");
break;
case 3:
Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
break;
default:
Serial.print("Unknown error ");
Serial.print(error);
Serial.println(".");
}
type = ps2x.readType();
switch(type) {
case 0:
Serial.println("Unknown Controller type");
break;
case 1:
Serial.println("DualShock Controller Found");
break;
case 2:
Serial.println("GuitarHero Controller Found");
break;
}
ps2x.read_gamepad(); // initialisation des valeurs
delay(50);
ps2x.read_gamepad(); /* initialise les états */
}
void loopRemote() {
static uint32_t chrono = 0;
const uint32_t periode = 20ul; // la manette est assez sensible, il ne faut pas lui demander les infos trop souvent. 15ms ou 20ms ça fonctionne pour moi
if (millis() - chrono >= periode) {
int RX = 0, RY = 0, LX = 0, LY = 0;
static int PRX = 127, PRY = 128, PLX = 127, PLY = 128;
chrono += periode;
ps2x.read_gamepad(false, vibrate); /* on lit l'état de la manette */
LX = ps2x.Analog(PSS_LX); /* joystick de gauche X*/
LY = ps2x.Analog(PSS_LY); /* joystick de gauche Y */
MotionPadX = LX;
MotionPadY = LY;
RX = ps2x.Analog(PSS_RX); /* joystick de droite X */
RY = ps2x.Analog(PSS_RY); /* joystick de droite Y */
// si une des coordonnées des joystick a changé alors on imprime les nouvelles coordonnées
if ((LX != PLX) || (LY != PLY) || (RX != PRX) || (RY != PRY)) {
PLX = LX;
PLY = LY;
PRX = RX;
PRY = RY;
Serial.print(LX); Serial.print(F(" ,"));
Serial.print(LY); Serial.print(F(" ,"));
Serial.print(RX); Serial.print(F(" ,"));
Serial.println(RY);
}
if (ps2x.ButtonPressed(PSB_START)) { Serial.println(F("START APPUI")); vibrate = 255; }
if (ps2x.ButtonPressed(PSB_SELECT)) {
Serial.println(F("SELECT APPUI"));
MOTION_MECANUM = !MOTION_MECANUM;
}
if (ps2x.ButtonPressed(PSB_PAD_UP)) {
Serial.println(F("HAUT APPUI"));
MotionMove = __MOTION_FORWARD;
}
if (ps2x.ButtonPressed(PSB_PAD_RIGHT)) {
Serial.println(F("DROIT APPUI"));
MotionMove = __MOTION_GORIGHT;
}
if (ps2x.ButtonPressed(PSB_PAD_LEFT)){
Serial.println(F("GAUCHE APPUI"));
MotionMove = __MOTION_GOLEFT;
}
if (ps2x.ButtonPressed(PSB_PAD_DOWN)) {
Serial.println(F("BAS APPUI"));
MotionMove = __MOTION_BACKWARD;
}
if (ps2x.ButtonPressed(PSB_L1)) Serial.println(F("GAUCHE 1 APPUI"));
if (ps2x.ButtonPressed(PSB_R1)) Serial.println(F("DROITE 1 APPUI"));
if (ps2x.ButtonPressed(PSB_L2)) Serial.println(F("GAUCHE 2 APPUI"));
if (ps2x.ButtonPressed(PSB_R2)) Serial.println(F("DROITE 2 APPUI"));
if (ps2x.ButtonPressed(PSB_L3)) Serial.println(F("ANALOG GAUCHE APPUI"));
if (ps2x.ButtonPressed(PSB_R3)) Serial.println(F("ANALOG DROITE APPUI"));
if (ps2x.ButtonPressed(PSB_GREEN)) {
Serial.println(F("Triangle APPUI"));
MOTION_TURBO = true;
}
if (ps2x.ButtonPressed(PSB_BLUE)) Serial.println(F("X APPUI"));
if (ps2x.ButtonPressed(PSB_RED)) Serial.println(F("Cercle APPUI"));
if (ps2x.ButtonPressed(PSB_PINK)) Serial.println(F("Carre APPUI"));
if (ps2x.Button(PSB_START)) Serial.println(F("START"));
if (ps2x.Button(PSB_SELECT)) Serial.println(F("SELECT"));
if (ps2x.Button(PSB_PAD_UP)) Serial.println(F("HAUT"));
if (ps2x.Button(PSB_PAD_RIGHT)) Serial.println(F("DROITE"));
if (ps2x.Button(PSB_PAD_LEFT)) Serial.println(F("GAUCHE"));
if (ps2x.Button(PSB_PAD_DOWN)) Serial.println(F("BAS"));
if (ps2x.Button(PSB_L1)) Serial.println(F("GAUCHE 1"));
if (ps2x.Button(PSB_R1)) Serial.println(F("DROITE 1"));
if (ps2x.Button(PSB_L2)) Serial.println(F("GAUCHE 2"));
if (ps2x.Button(PSB_R2)) Serial.println(F("DROITE 2"));
if (ps2x.Button(PSB_L3)) Serial.println(F("ANALOG GAUCHE"));
if (ps2x.Button(PSB_R3)) Serial.println(F("ANALOG DROITE"));
if (ps2x.Button(PSB_GREEN)) Serial.println(F("Triangle"));
if (ps2x.Button(PSB_BLUE)) Serial.println(F("X"));
if (ps2x.Button(PSB_RED)) Serial.println(F("Cercle"));
if (ps2x.Button(PSB_PINK)) Serial.println(F("Carre"));
if (ps2x.ButtonReleased(PSB_START)) { Serial.println(F("START RELACHE")); vibrate = 0; }
if (ps2x.ButtonReleased(PSB_SELECT)) Serial.println(F("SELECT RELACHE"));
if (ps2x.ButtonReleased(PSB_PAD_UP)) {
Serial.println(F("HAUT RELACHE"));
MotionMove = __MOTION_STOP;
}
if (ps2x.ButtonReleased(PSB_PAD_RIGHT)) {
Serial.println(F("DROITE RELACHE"));
MotionMove = __MOTION_STOP;
}
if (ps2x.ButtonReleased(PSB_PAD_LEFT)) {
Serial.println(F("GAUCHE RELACHE"));
MotionMove = __MOTION_STOP;
}
if (ps2x.ButtonReleased(PSB_PAD_DOWN)) {
Serial.println(F("BAS RELACHE"));
MotionMove = __MOTION_STOP;
}
if (ps2x.ButtonReleased(PSB_L1)) Serial.println(F("GAUCHE 1 RELACHE"));
if (ps2x.ButtonReleased(PSB_R1)) Serial.println(F("DROITE 1 RELACHE"));
if (ps2x.ButtonReleased(PSB_L2)) Serial.println(F("GAUCHE 2 RELACHE"));
if (ps2x.ButtonReleased(PSB_R2)) Serial.println(F("DROITE 2 RELACHE"));
if (ps2x.ButtonReleased(PSB_L3)) Serial.println(F("ANALOG GAUCHE RELACHE"));
if (ps2x.ButtonReleased(PSB_R3)) Serial.println(F("ANALOG DROITE RELACHE"));
if (ps2x.ButtonReleased(PSB_GREEN)) {
Serial.println(F("Triangle RELACHE"));
MOTION_TURBO = false;
}
if (ps2x.ButtonReleased(PSB_BLUE)) Serial.println(F("X RELACHE"));
if (ps2x.ButtonReleased(PSB_RED)) Serial.println(F("Cercle RELACHE"));
if (ps2x.ButtonReleased(PSB_PINK)) Serial.println(F("Carre RELACHE"));
}
}
#endif
#ifndef __ROBOTCLEANER_MOTION_H__
#define __ROBOTCLEANER_MOTION_H__
#define __MOTION_DEBUG_ false
#include <Arduino.h>
#include "remote.h"
#include <AFMotor.h>
//Constants
const int slowSpeed=150;
const int fastSpeed=250;
//Parameters
AF_DCMotor motorFrontLeft(1);
AF_DCMotor motorFrontRight(2);
AF_DCMotor motorBackLeft(3);
AF_DCMotor motorBackRight(4);
int motorFrontLeftSpeed = 0;
int motorFrontRightSpeed = 0;
int motorBackLeftSpeed = 0;
int motorBackRightSpeed = 0;
void setupMotion() {
if (__MOTION_DEBUG_) {
Serial.println("==> Test 4 Motors ...");
motorFrontLeft.setSpeed(255);
motorFrontLeft.run(FORWARD);
motorFrontRight.setSpeed(255);
motorFrontRight.run(FORWARD);
motorBackLeft.setSpeed(255);
motorBackLeft.run(FORWARD);
motorBackRight.setSpeed(255);
motorBackRight.run(FORWARD);
delay(2000);
}
motorFrontLeft.setSpeed(0);
motorFrontLeft.run(RELEASE);
motorFrontRight.setSpeed(0);
motorFrontRight.run(RELEASE);
motorBackLeft.setSpeed(0);
motorBackLeft.run(RELEASE);
motorBackRight.setSpeed(0);
motorBackRight.run(RELEASE);
Serial.println("Motors initialised.");
}
void loopMotion() {
int joystickXValue = 0;
int joystickYValue = 0;
if(MOTION_TURBO) {
joystickXValue = map(MotionPadX, 0,255,-128,128);
joystickYValue = map(MotionPadY, 0,255,-128,128);
} else {
joystickXValue = map(MotionPadX, 0,255,-255,255);
joystickYValue = map(MotionPadY, 0,255,-255,255);
}
if(joystickXValue>-10 & joystickXValue<10) {
joystickXValue = 0;
}
if(joystickYValue>-10 & joystickYValue<10) {
joystickYValue = 0;
}
if(MOTION_MECANUM) {
// conversion des valeurs analogiques du joystick en vitesses pour les moteurs
motorBackLeftSpeed = joystickYValue - joystickXValue;
motorBackRightSpeed = joystickYValue + joystickXValue;
motorFrontLeftSpeed = motorBackLeftSpeed; // joystickYValue + joystickXValue;
motorFrontRightSpeed = motorBackRightSpeed; // joystickYValue - joystickXValue;
// limitation des vitesses maximale et minimale des moteurs
motorFrontLeftSpeed = constrain(motorFrontLeftSpeed, -255, 255);
motorFrontRightSpeed = constrain(motorFrontRightSpeed, -255, 255);
motorBackLeftSpeed = constrain(motorBackLeftSpeed, -255, 255);
motorBackRightSpeed = constrain(motorBackRightSpeed, -255, 255);
// envoi des vitesses aux moteurs
motorFrontLeft.setSpeed(abs(motorFrontLeftSpeed));
motorFrontRight.setSpeed(abs(motorFrontRightSpeed));
motorBackLeft.setSpeed(abs(motorBackLeftSpeed));
motorBackRight.setSpeed(abs(motorBackRightSpeed));
// inversion des directions des moteurs si nécessaire
if (motorFrontLeftSpeed < 0) {
motorFrontLeft.run(BACKWARD);
} else {
motorFrontLeft.run(FORWARD);
}
if (motorFrontRightSpeed < 0) {
motorFrontRight.run(BACKWARD);
} else {
motorFrontRight.run(FORWARD);
}
if (motorBackLeftSpeed < 0) {
motorBackLeft.run(BACKWARD);
} else {
motorBackLeft.run(FORWARD);
}
if (motorBackRightSpeed < 0) {
motorBackRight.run(BACKWARD);
} else {
motorBackRight.run(FORWARD);
}
} else {
// conversion des valeurs analogiques du joystick en vitesses pour les moteurs
motorFrontLeftSpeed = joystickYValue + joystickXValue;
motorFrontRightSpeed = joystickYValue - joystickXValue;
motorBackLeftSpeed = joystickYValue - joystickXValue;
motorBackRightSpeed = joystickYValue + joystickXValue;
// limitation des vitesses maximale et minimale des moteurs
motorFrontLeftSpeed = constrain(motorFrontLeftSpeed, -255, 255);
motorFrontRightSpeed = constrain(motorFrontRightSpeed, -255, 255);
motorBackLeftSpeed = constrain(motorBackLeftSpeed, -255, 255);
motorBackRightSpeed = constrain(motorBackRightSpeed, -255, 255);
// envoi des vitesses aux moteurs
motorFrontLeft.setSpeed(abs(motorFrontLeftSpeed));
motorFrontRight.setSpeed(abs(motorFrontRightSpeed));
motorBackLeft.setSpeed(abs(motorBackLeftSpeed));
motorBackRight.setSpeed(abs(motorBackRightSpeed));
// inversion des directions des moteurs si nécessaire
if (motorFrontLeftSpeed < 0) {
motorFrontLeft.run(BACKWARD);
} else {
motorFrontLeft.run(FORWARD);
}
if (motorFrontRightSpeed < 0) {
motorFrontRight.run(BACKWARD);
} else {
motorFrontRight.run(FORWARD);
}
if (motorBackLeftSpeed < 0) {
motorBackLeft.run(BACKWARD);
} else {
motorBackLeft.run(FORWARD);
}
if (motorBackRightSpeed < 0) {
motorBackRight.run(BACKWARD);
} else {
motorBackRight.run(FORWARD);
}
}
}
#endif
#ifndef __ROBOTCLEANER_CAMERA_H__
#define __ROBOTCLEANER_CAMERA_H__
#define __CAMERA_DEBUG_ false
#include <Arduino.h>
#include "HUSKYLENS.h"
#include "Wire.h"
void switchConfig(int);
void printResult(HUSKYLENSResult);
HUSKYLENS huskylens;
int currentID = 4; // 4 = Cube (ObjectTracking_Backup_4.bin), 2 = Bag (ObjectTracking_Backup_2.bin)
unsigned long lastSwitchTime = 0;
const unsigned long TIMEOUT = 30000; // Temps d'attente avant de changer de cible (5s)
void setupCamera() {
Wire.begin();
while (!huskylens.begin(Wire)) {
Serial.println(F("HuskyLens non détectée..."));
delay(1000);
}
// Initialisation en mode Tracking
huskylens.writeAlgorithm(ALGORITHM_OBJECT_TRACKING);
// Chargement de la première configuration
switchConfig(currentID);
}
void loopCamera() {
bool objectFound = false;
if (!huskylens.request()) {
Serial.println(F("Erreur de communication."));
}
else if (huskylens.available()) {
while (huskylens.available()) {
HUSKYLENSResult result = huskylens.read();
printResult(result);
if (result.command == COMMAND_RETURN_BLOCK) {
objectFound = true;
}
}
}
}
void switchConfig(int id) {
Serial.print(F("Recherche de l'objet "));
Serial.println(id == 4 ? F("CUBE...") : F("BAG..."));
// La caméra va chercher ObjectTracking_Backup_2.bin ou ObjectTracking_Backup_4.bin à la racine de la SD
huskylens.loadModelFromSDCard(id);
Serial.println(F("Fichier chargé."));
if (id == 4) {
huskylens.setCustomName("CUBE", 1);
} else {
huskylens.setCustomName("BAG", 1);
}
// On attend un peu que la caméra stabilise l'image après chargement
delay(8000);
if (!huskylens.isLearned()) {
Serial.println(F("Erreur : HUSKYLENS have NOT learn something !"));
} else {
Serial.println(F("HUSKYLENS have learn something"));
}
}
void printResult(HUSKYLENSResult result){
if (result.command == COMMAND_RETURN_BLOCK){
Serial.println(String()+F("Block:xCenter=")+result.xCenter+F(",yCenter=")+result.yCenter+F(",width=")+result.width+F(",height=")+result.height+F(",ID=")+(currentID == 4 ? F("CUBE...") : F("BAG...")));
}
else if (result.command == COMMAND_RETURN_ARROW){
Serial.println(String()+F("Arrow:xOrigin=")+result.xOrigin+F(",yOrigin=")+result.yOrigin+F(",xTarget=")+result.xTarget+F(",yTarget=")+result.yTarget+F(",ID=")+(currentID == 4 ? F("CUBE...") : F("BAG...")));
}
else{
Serial.println("Object unknown!");
}
}
#endif
#ifndef __ROBOTCLEANER_NIPPERS_H__
#define __ROBOTCLEANER_NIPPERS_H__
#define __NIPPERS_DEBUG_ false
#include <Arduino.h>
void setupNippers() {
}
void loopNippers() {
}
#endif
¶ Principe des roues Mecanum (Suédoises)
¶ Fichiers Solidworks 2023
| Pièce | Fichier | Notes |
|---|---|---|
| Pièce à fixer ... | ||
| Pièce à fixer ... |