CRAC Team
pour trouver l'automate de reception can
Fork of
CRAC-Strat_copy
by 
Clement Breteau
Revision 15:c2fc239e85df, committed 2017-05-11
- Comitter:
- ClementBreteau
- Date:
- Thu May 11 12:55:52 2017 +0000
- Parent:
- 14:c8fc06c4887f
- Commit message:
- crac strat le 11/05/17
Changed in this revision
--- a/AX12-V2/AX12-V2.cpp Fri Mar 31 16:20:26 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,672 +0,0 @@
-#include "AX12-V2.h"
-
-struct S_AX12 AX12_data[MAX_AX12];//La liste de tous les AX12 du robot possible, aussi bien par CAN que en local
-
-SerialHalfDuplex AX12_Serial1 = SerialHalfDuplex(p9,p10);
-SerialHalfDuplex AX12_Serial2 = SerialHalfDuplex(p28,p27);
-
-int lastAX12Use = 0;
-
-FunctionPointer AX12_CallbackEnd;//Fonction de callback lors de la fin d'un mouvement d'un AX12
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_register */
-/* DESCRIPTION : Indiquer qu'un AX12 est connecté à la carte */
-/****************************************************************************************/
-void AX12_register(unsigned char id, unsigned char serial, unsigned short speed)
-{
- int localID = AX12_getLocalID(id);
-
- AX12_data[localID].isUsingCAN = 0;//On indique que l'AX12 est connecté localement
-
- if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023
-
- AX12_data[localID].speed = speed;
- AX12_data[localID].serial = serial;
- //printf("registering AX12 id: %d local: %d\n",id,localID);
- AX12_Serial1.baud(1000000);//On indique la vitesse de transmission des AX12
- AX12_Serial2.baud(1000000);//On indique la vitesse de transmission des AX12
-
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_setGoal */
-/* DESCRIPTION : Definir la position d'un ax12, !!Ne déclanche pas le mouvement */
-/****************************************************************************************/
-void AX12_setGoal(unsigned char id, unsigned short goal, unsigned short speed)
-{
- int localID = AX12_getLocalID(id);//On récupère les info sur l'AX12
- CANMessage msgTx=CANMessage();
-
- AX12_data[localID].needToUpdate = 1;
- AX12_data[localID].goal = goal;
- if(speed > 0x3FF) speed = 0x3FF;//La vitesse ne doit pas depasser 1023
- if(speed != 0x3FF)
- AX12_data[localID].speed = speed;
-
- if(AX12_data[localID].isUsingCAN != 0) {//Il faut envoyer la trame CAN car l'AX12 est sur une autre carte
- msgTx.id=SERVO_AX12_SETGOAL;
- msgTx.len=5;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- // id de l'AX12 sur 1 octet
- msgTx.data[0]=(unsigned char)id;
- // Position de l'AX12 sur 2 octet
- msgTx.data[1]=(unsigned char)goal;
- msgTx.data[2]=(unsigned char)(goal>>8);
- //Vitesse de l'AX12 sur 2 octet
- msgTx.data[3]=(unsigned char)AX12_data[localID].speed;
- msgTx.data[4]=(unsigned char)(AX12_data[localID].speed>>8);
-
- can1.write(msgTx);
- }
-
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_isLocal */
-/* DESCRIPTION : Savoir si un AX12 est enregistré sur la carte */
-/****************************************************************************************/
-unsigned char AX12_isLocal(unsigned char id)
-{
- int i=0;
- for(i=0;i<MAX_AX12;i++)
- {
- if(AX12_data[i].id == id && AX12_data[i].isUsingCAN == 0) return 1;
- }
- return 0;
-}
-
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_getLocalID */
-/* DESCRIPTION : Obtenir les info sur un AX12 ou l'initialiser si non présent */
-/****************************************************************************************/
-int AX12_getLocalID(unsigned char id)
-{
- int i=0;
- for(i=0;i<lastAX12Use;i++)
- {
- if(AX12_data[i].id == id) return i;
- }
- //Si l'AX12 n'est pas déjà initialisé, on l'initialise
- AX12_data[lastAX12Use].id = id;//
- AX12_data[lastAX12Use].goal = 0;//
- AX12_data[lastAX12Use].speed = 0x320;//
- AX12_data[lastAX12Use].isUsingCAN = 1;//Indique qu'il faut envoyer le message via CAN
- AX12_data[lastAX12Use].needToUpdate = 0;//
- lastAX12Use++;
- return lastAX12Use-1;
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_notifyCANEnd */
-/* DESCRIPTION : indiquer qu'un mouvement d'AX12 CAN est terminé */
-/****************************************************************************************/
-void AX12_notifyCANEnd(unsigned char id)
-{
- if(waitingAckFrom == SERVO_AX12_DONE && waitingAckID == id) {
- waitingAckFrom = 0;
- waitingAckID = 0;
- }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_doLoop */
-/* DESCRIPTION : Boucle de vérification de la position des AX12 */
-/****************************************************************************************/
-void AX12_doLoop(void)
-{
- int i=0;
- CANMessage msgTx=CANMessage();
-
- for(i=0;i<lastAX12Use;i++)
- {
- if(AX12_data[i].isUsingCAN == 0 && AX12_data[i].needCheckMoving == 1)//Il faut vérifier si l'AX12 a terminé de bouger
- {
- if(AX12_isMoving(i) == 0) {//L'AX12 a terminé de bouger
- AX12_data[i].needCheckMoving = 0;
-
- msgTx.id=SERVO_AX12_DONE;
- msgTx.len=1;
- msgTx.format=CANStandard;
- msgTx.type=CANData;
- // id de l'AX12 sur 1 octet
- msgTx.data[0]=(unsigned char)AX12_data[i].id;
- can1.write(msgTx);
- AX12_notifyCANEnd(AX12_data[i].id);
- }
- }
- }
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_processChange */
-/* DESCRIPTION : Permet de prendre en compte les changement d'instruction des AX12 */
-/* Début du mouvement à partir de l'appel de cette fonction */
-/****************************************************************************************/
-void AX12_processChange(unsigned char fromCan)
-{
- int i=0;
- int dataToSendLength = 0;
- char dataToSend[100];
- int sendTwice = 0;
-
- for(i=0;i<lastAX12Use;i++)
- {
- //printf("checking AX12 id: %d",AX12_data[i].id);
- if(AX12_data[i].needToUpdate == 1) //Il faut mettre à jour la position de l'AX12
- {
- //printf(" => update");
- if(AX12_data[i].isUsingCAN == 0)//Il faut envoyer la trame en local
- {
- //printf(" local");
- if(dataToSendLength == 0)
- dataToSend[dataToSendLength++] = 4;//length data
- dataToSend[dataToSendLength++] = AX12_data[i].id;//ID servo1
- dataToSend[dataToSendLength++] = ((1023 * AX12_data[i].goal) / 300) & 0xff;// bottom 8 bits
- dataToSend[dataToSendLength++] = ((1023 * AX12_data[i].goal) / 300) >> 8; // top 8 bits
- dataToSend[dataToSendLength++] = (AX12_data[i].speed) & 0xff;// bottom 8 bits
- dataToSend[dataToSendLength++] = (AX12_data[i].speed) >> 8; // top 8 bits
- AX12_data[i].needCheckMoving = 1;
- }
- AX12_data[i].needToUpdate = 0;//Remise à 0 de l'indicatif de mise à jour
- }
- //printf("\n");
- }
- if(fromCan == 0)
- SendRawId(SERVO_AX12_PROCESS);//On indique par CAN qu'il faut bouger les AX12
- //printf("need to send %d data\n",dataToSendLength);
- if(dataToSendLength > 0)//Il y a des données à envoyer en local
- {
- for(sendTwice=0;sendTwice<2;sendTwice++)
- {
- AX12_syncWrite(AX12_Serial1, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
- //wait_ms(10);
- AX12_syncWrite(AX12_Serial2, AX12_REG_GOAL_POSITION, dataToSendLength, dataToSend);
- }
- }
- SendRawId(0x456);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_isMoving */
-/* DESCRIPTION : Fonction pour savoir si un AX12 local est entrain de bouger */
-/****************************************************************************************/
-int AX12_isMoving(unsigned char id)
-{
- char data[1];
- if(AX12_data[id].serial == 0)
- AX12_read(AX12_Serial1,AX12_data[id].id,AX12_REG_MOVING,1,data);
- else
- AX12_read(AX12_Serial2,AX12_data[id].id,AX12_REG_MOVING,1,data);
- return(data[0]);
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_syncWrite */
-/* DESCRIPTION : Fonction pour envoyer des trames aux AX12 en mode syncWrite */
-/****************************************************************************************/
-int AX12_syncWrite(SerialHalfDuplex& AX12_Serial, int start, int bytes, char* data)
-{
-//0 : 0xff
-//1 : 0xff
-//2 : ID de l'AX12 ou 0xFE pour le broadcast
-//3 : Length => longueur de la trame
-//4 : Intruction(write) => id de l'instruction 0x83 pour le syncwrite
-//5 : Address => addresse du registre à modifier
-//6+ : Data => les données à transmettre
-//last : Checksum
- int ID = 0xFE;//Toujours 0xFE dans le cas d'un broadcast
-
- char TxBuf[60];
- char sum = 0;
-
- int timeout_transmit = 0;
- int i = 0;
- //printf("Start sending moving trame\n");
- // Build the TxPacket first in RAM, then we'll send in one go
-
- TxBuf[0] = 0xff;
- TxBuf[1] = 0xff;
-
- // ID
- TxBuf[2] = ID;
- sum += TxBuf[2];
-
- // packet Length
- TxBuf[3] = 3+bytes;
- sum += TxBuf[3];
-
-
- // Instruction
- TxBuf[4]=0x83;//toujours 0x83 dans le cas d'un syncwrite
- sum += TxBuf[4];
-
-
- // Start Address
- TxBuf[5] = start;//addresse du registre à modifier
- sum += TxBuf[5];
-
- // data
- for (char i=0; i<bytes ; i++) {
- TxBuf[6+i] = data[i];
- sum += TxBuf[6+i];
- //printf(" Data : 0x%x\n",TxBuf[6+i]);
- }
-
- // checksum
- TxBuf[6+bytes] = 0xFF - sum;
-
-
- /* Transmission de la trame construite precedemment dans le tableau TxBuf
- */
- while ((timeout_transmit<100) && (i < (7+bytes))) {
- if (AX12_Serial.writeable()) {
- AX12_Serial.putc(TxBuf[i]);
- i++;
- timeout_transmit = 0;
- } else timeout_transmit++;
- }
-
- if (timeout_transmit == 100 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
- return(-1);
- }
-
- // Wait for data to transmit
- wait (0.005);
- return(0);//OK trame envoyé
-}
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_write */
-/* DESCRIPTION : Fonction pour envoyer des trames aux AX12 */
-/****************************************************************************************/
-int AX12_write(SerialHalfDuplex& AX12_Serial, int ID, int start, int bytes, char* data, int flag)
-{
-// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
-// = AX12_Serial1;
- char TxBuf[16];
- char sum = 0;
- char Status[6];
-
- int timeout = 0;
- int plen = 0;
- int flag_out = 0;
- int timeout_transmit = 0;
- int i = 0;
- /*int poubelle = 0;
- int count = 0;
- char vidage[50];*/
-
- typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
- type_etat etat = Header1;
-
- // Build the TxPacket first in RAM, then we'll send in one go
- TxBuf[0] = 0xff;
- TxBuf[1] = 0xff;
-
- // ID
- TxBuf[2] = ID;
- sum += TxBuf[2];
-
- // packet Length
- TxBuf[3] = 3+bytes;
- sum += TxBuf[3];
-
- // Instruction
- if (flag == 1) {
- TxBuf[4]=0x04;
- sum += TxBuf[4];
- } else {
- TxBuf[4]=0x03;
- sum += TxBuf[4];
- }
-
- // Start Address
- TxBuf[5] = start;
- sum += TxBuf[5];
-
-
- // data
- for (char i=0; i<bytes ; i++) {
- TxBuf[6+i] = data[i];
- sum += TxBuf[6+i];
- }
-
- // checksum
- TxBuf[6+bytes] = 0xFF - sum;
-
-
-
- /* Transmission de la trame construite precedemment dans le tableau TxBuf
- */
- while ((timeout_transmit<100) && (i < (7+bytes))) {
- if (AX12_Serial.writeable()) {
- AX12_Serial.putc(TxBuf[i]);
- i++;
- timeout_transmit = 0;
- } else timeout_transmit++;
- }
-
- if (timeout_transmit == 100 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
- return(-1);
- }
- /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
- */
-
-
- // Wait for data to transmit
- wait (0.005);
-
- // make sure we have a valid return
- Status[4]=0x00;
-
- // we'll only get a reply if it was not broadcast
- if (ID!=0xFE) {
-
-
- /* Partie de reception de la trame de retour
- */
- while ((flag_out != 1) && (timeout < MAX_TIMEOUT)) {
- // Les differents etats de l'automate on été créés au debut de la fonction write !
- switch (etat) {
- case Header1:
- if (AX12_Serial.readable()) { // reception du premier Header ( 0xFF )
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- if (Status[plen] == 0xFF ) {
- etat = Header2;
- plen++;
-
- } else etat = Header1;
- } else timeout++;
- break;
-
-
- case Header2:
- if (AX12_Serial.readable()) { // reception du second Header ( 0xFF )
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- if (Status[plen] == 0xFF ) {
- etat = ident;
- plen++;
- } else {
- etat = Header1;
- plen = 0;
- }
- } else timeout++;
- break;
-
- case ident:
- if (AX12_Serial.readable()) { // reception de l'octet correspondant à l'ID du servomoteur
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- if (Status[plen] == ID ) {
- etat = length;
- plen++;
- } else {
- etat = Header1;
- plen = 0;
- }
- } else timeout++;
- break;
-
- case length:
- if (AX12_Serial.readable()) { // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- if (Status[plen] == 2 ) { // dans la trame de retour d'un write il n'y a pas de paramètre la taille vaudra donc 2!!
- etat = erreur;
- plen++;
- } else {
- etat = Header1;
- plen = 0;
- }
- } else timeout++;
- break;
-
- case erreur:
- if (AX12_Serial.readable()) { //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- plen++;
- etat = checksum;
- } else timeout++;
-
- case checksum:
- if (AX12_Serial.readable()) { // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- flag_out = 1;
- etat = Header1;
- } else timeout++;
- break;
- }
- }
-
- if (timeout == MAX_TIMEOUT ) { // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames
- return(-1);
- }
-
- // Build the TxPacket first in RAM, then we'll send in one go
- }
-
- return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
-}
-
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_read */
-/* DESCRIPTION : Lire des données dans un registre de l'AX12 */
-/****************************************************************************************/
-int AX12_read(SerialHalfDuplex& AX12_Serial, int ID, int start, int bytes, char* data)
-{
-
-//SerialHalfDuplex AX12_Serial = AX12_Serial1;
- char PacketLength = 0x3;
- char TxBuf[16];
- char sum = 0;
- char Status[16];
-
- int timeout = 0;
- int plen = 0;
- int flag_out = 0;
- int timeout_transmit = 0;
- int i = 0;
- int enable = 0;
-// int poubelle = 0;
-// int count = 0;
-// char vidage[50];
-
- typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat;
- type_etat etat = Header1;
-
- Status[4] = 0xFE; // return code
-
-
-
-
-
- /*********************************** CREATION DE LA TRAME A EVOYER *****************************************/
-
-
- // Build the TxPacket first in RAM, then we'll send in one go
-
- TxBuf[0] = 0xff;
- TxBuf[1] = 0xff;
-
- // ID
- TxBuf[2] = ID;
- sum += TxBuf[2];
-
- // Packet Length
- TxBuf[3] = PacketLength+bytes; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
- sum += TxBuf[3]; // Accululate the packet sum
-
-
- // Instruction - Read
- TxBuf[4] = 0x2;
- sum += TxBuf[4];
-
- // Start Address
- TxBuf[5] = start;
- sum += TxBuf[5];
-
- // Bytes to read
- TxBuf[6] = bytes;
- sum += TxBuf[6];
-
- // Checksum
- TxBuf[7] = 0xFF - sum;
-
- /********************************************TRAME CONSTRUITE DANS TxBuf***************************************/
-
-
-
-
- /* Transmission de la trame construite precedemment dans le tableau TxBuf
- */
- while ((timeout_transmit<1000) && (i < (7+bytes))) {
- if (AX12_Serial.writeable()) {
- AX12_Serial.putc(TxBuf[i]);
- i++;
- timeout_transmit = 0;
- } else timeout_transmit++;
- }
-
- if (timeout_transmit == 1000 ) { // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
- return(-1);
- }
- /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
- */
- // Wait for the bytes to be transmitted
- wait (0.001);
-
- // Skip if the read was to the broadcast address
- if (ID != 0xFE) {
-
- /* Partie de reception de la trame de retour
- */
- while ((flag_out != 1) && (timeout < (1000*bytes))) {
- // Les differents etats de l'automate on été créés au debut de la fonction write !
- switch (etat) {
- case Header1:
- if (AX12_Serial.readable()) { // reception du premier Header ( 0xFF )
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- if (Status[plen] == 0xFF ) {
- etat = Header2;
- plen++;
-
- } else etat = Header1;
- } else timeout++;
- break;
-
-
- case Header2:
- if (AX12_Serial.readable()) { // reception du second Header ( 0xFF )
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- if (Status[plen] == 0xFF ) {
- etat = ident;
- plen++;
-
- } else if (Status[plen] == ID ) { // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!!
- Status[plen] = 0;
- plen++;
- Status[plen] = ID;
- etat = length;
- plen++;
-
- } else {
-
- etat = Header1;
- plen = 0;
- }
- } else timeout++;
- break;
-
- case ident:
- if (AX12_Serial.readable()) { // reception de l'octet correspondant à l'ID du servomoteur
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- if (Status[plen] == ID ) {
- etat = length;
- plen++;
-
- } else {
- etat = Header1;
- plen = 0;
- }
- } else timeout++;
- break;
-
- case length:
- if (AX12_Serial.readable()) { // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- if (Status[plen] == (bytes+2) ) {
- etat = erreur;
- plen++;
-
- } else {
- etat = Header1;
- plen = 0;
- }
- } else timeout++;
- break;
-
- case erreur:
- if (AX12_Serial.readable()) { //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- plen++;
-
- etat = reception;
- } else timeout++;
-
- case reception:
- while ( enable < bytes ) { // reception du ou des octect(s) de donnés ( suivant la valeur de la variable length )
- if (AX12_Serial.readable()) {
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- plen++;
- enable++;
-
- } else timeout++;
- }
- etat = checksum;
- break;
-
- case checksum:
- if (AX12_Serial.readable()) { // reception du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length + somme des données ) >>>> dans le cas d'un retour d'un read!!
- Status[plen] = AX12_Serial.getc();
- timeout = 0;
- flag_out = 1;
- etat = Header1;
- } else timeout++;
- break;
-
- default:
- break;
- }
- }
-
-
- if (timeout == (1000*bytes) ) { // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames
- return(-1);
- }
-
-
- // copie des données dans le tableau data
- for (int i=0; i < Status[3]-2 ; i++) {
- data[i] = Status[5+i];
- }
-
- } // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast ID (ID!=0xFE)
-
- return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
-}
-
-
--- a/AX12-V2/AX12-V2.h Fri Mar 31 16:20:26 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,150 +0,0 @@
-#ifndef CRAC_AX12
-#define CRAC_AX12
-
-#include "global.h"
-
-#define MAX_AX12 30
-
-
-#define AX12_REG_ID 0x03
-#define AX12_REG_BAUD 0x04
-#define AX12_REG_DELAY_TIME 0x05
-#define AX12_REG_CW_LIMIT 0x06
-#define AX12_REG_CCW_LIMIT 0x08
-#define AX12_REG_TEMP_MAX 0x0B
-#define AX12_REG_LOWEST_VOLTAGE 0x0C
-#define AX12_REG_HIGHEST_VOLTAGE 0x0D
-#define AX12_REG_MAX_TORQUE 0x0E
-#define AX12_REG_SATUS_RETURN 0x10
-#define AX12_REG_ALARM_LED 0x11
-#define AX12_REG_ALARM_SHUTDOWN 0x12
-#define AX12_REG_DOWN_CALIBRATION 0x14
-#define AX12_REG_UP_CALIBRATION 0x16
-#define AX12_REG_TORQUE_ENABLE 0x18
-#define AX12_REG_LED 0x19
-#define AX12_REG_CW_MARGIN 0x1A
-#define AX12_REG_CCW_MARGIN 0x1B
-#define AX12_REG_CW_SLOPE 0x1C
-#define AX12_REG_CCW_SLOPE 0x1D
-#define AX12_REG_GOAL_POSITION 0x1E
-#define AX12_REG_MOVING_SPEED 0x20
-#define AX12_REG_TORQUE_LIMIT 0x22
-#define AX12_REG_POSITION 0x24
-#define AX12_REG_PRESENT_SPEED 0x26
-#define AX12_REG_PRESENT_LOAD 0x28
-#define AX12_REG_VOLTS 0x2A
-#define AX12_REG_TEMP 0x2B
-#define AX12_REG_INSTRUCTION 0x2C
-#define AX12_REG_MOVING 0x2E
-#define AX12_REG_LOCK 0x2F
-#define AX12_REG_PUNCH 0x30
-
-
-#define AX12_MODE_POSITION 0
-#define AX12_MODE_ROTATION 1
-
-#define AX12_CW 1
-#define AX12_CCW 0
-
-//--- Instruction ---
-#define INST_PING 0x01
-#define INST_READ 0x02
-#define INST_WRITE 0x03
-#define INST_REG_WRITE 0x04
-#define INST_ACTION 0x05
-#define INST_RESET 0x06
-#define INST_DIGITAL_RESET 0x07
-#define INST_SYSTEM_READ 0x0C
-#define INST_SYSTEM_WRITE 0x0D
-#define INST_SYNC_WRITE 0x83
-#define INST_SYNC_REG_WRITE 0x84
-
-#define DEFAULT_RETURN_PACKET_SIZE 6
-
-#define BROADCASTING_ID 0xfe
-
-#define MAX_TIMEOUT 5000
-
-#define AX12_SERIAL1 0
-#define AX12_SERIAL2 1
-
-
-struct S_AX12 {
- unsigned char id;//L'id de l'AX12
- unsigned short goal;//La position ou doit aller le robot
- unsigned short speed;//La vitesse de déplacement du bras
- unsigned char isUsingCAN;//Indique si il faut envoyer les information via CAN (0=>non | autre=>oui)
- unsigned char needToUpdate;//Indique qu'il faut mettre à jour la position
- unsigned char needCheckMoving;//Permet d'indiquer que l'on doit verifier si l'AX12 bouge
- unsigned char serial;
-};
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_register */
-/* DESCRIPTION : Indiquer qu'un AX12 est connecté à la carte */
-/****************************************************************************************/
-void AX12_register(unsigned char id,unsigned char serial, unsigned short speed = 0x3FF);
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_setGoal */
-/* DESCRIPTION : Definir la position d'un ax12, !!Ne déclanche pas le mouvement */
-/****************************************************************************************/
-void AX12_setGoal(unsigned char id, unsigned short goal, unsigned short speed = 0x3FF);
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_isLocal */
-/* DESCRIPTION : Savoir si un AX12 est enregistré sur la carte */
-/****************************************************************************************/
-unsigned char AX12_isLocal(unsigned char id);
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_notifyCANEnd */
-/* DESCRIPTION : indiquer qu'un mouvement d'AX12 CAN est terminé */
-/****************************************************************************************/
-void AX12_notifyCANEnd(unsigned char id);
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_getLocalID */
-/* DESCRIPTION : Obtenir les info sur un AX12 ou l'initialiser si non présent */
-/****************************************************************************************/
-int AX12_getLocalID(unsigned char id);
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_doLoop */
-/* DESCRIPTION : Boucle de vérification de la position des AX12 */
-/****************************************************************************************/
-void AX12_doLoop(void);
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_processChange */
-/* DESCRIPTION : Permet de prendre en compte les changement d'instruction des AX12 */
-/* Début du mouvement à partir de l'appel de cette fonction */
-/****************************************************************************************/
-void AX12_processChange(unsigned char fromCan = 0);
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_isMoving */
-/* DESCRIPTION : Fonction pour savoir si un AX12 local est entrain de bouger */
-/****************************************************************************************/
-int AX12_isMoving(unsigned char id);
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_syncWrite */
-/* DESCRIPTION : Fonction pour envoyer des trames aux AX12 en mode syncWrite */
-/****************************************************************************************/
-int AX12_syncWrite(SerialHalfDuplex& AX12_Serial,int start, int bytes, char* data);
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_write */
-/* DESCRIPTION : Fonction pour envoyer des trames aux AX12 */
-/****************************************************************************************/
-int AX12_write(SerialHalfDuplex& AX12_Serial, int ID, int start, int bytes, char* data, int flag=0);
-
-/****************************************************************************************/
-/* FUNCTION NAME: AX12_read */
-/* DESCRIPTION : Lire des données dans un registre de l'AX12 */
-/****************************************************************************************/
-int AX12_read(SerialHalfDuplex& AX12_Serial, int ID, int start, int bytes, char* data);
-
-#endif
-
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.cpp Thu May 11 12:55:52 2017 +0000
@@ -0,0 +1,1828 @@
+/* mbed AX-12+ Servo Library
+ *
+ * Copyright (c) 2010, cstyles (http://mbed.org)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include "mbed.h"
+#include "AX12.h"
+
+#define AX12_INITIALISATION 0
+#define AX12_PREPARATION_PRISE 1
+#define AX12_STOCKAGE_HAUT 2
+#define AX12_STOCKAGE_BAS 3
+#define AX12_DEPOSER 4
+#define AX12_PREPARATION_DEPOT_BAS 5
+#define AX12_PREPARATION_DEPOT_HAUT 6
+#define AX12_POUSSER_MODULE 7
+#define AX12_DEFAUT 20
+
+#define TIME 0.8
+#define SIZE_FIFO 25
+
+#define MAX_TIMEOUT 500
+
+extern "C" void mbed_reset();//Pour pouvoir reset la carte
+
+unsigned char action = 0, choix_bras = 0, etat_ax12 = 0, flag = 0, action_precedente = 0;
+short vitesse=700;
+float angle=0.0;
+float test_socle=0.0,test_bas=0.0,test_milieu=0.0,test_haut=0.0,test_ventouse=0.0, test_calcul=0.0, valeur_test=0.0;
+AX12 *un_myAX12, *sept_myAX12, *huit_myAX12, *seize_myAX12, *multiple_myAX12; //Pince centrale
+AX12 *un_myAX12_2, *deux_myAX12_2, *trois_myAX12_2, *multiple_myAX12_2; //Bras de gauche
+
+Timer t;
+Ticker flipper;
+
+////////////////////// TABLEAU PINCE CENTRALE ///////////////////////////
+static char TAB1[25]= {0x01,0x05, 0x02, 0xFF, 0x00, ///Position initiale
+ 0x10,0x00, 0x02, 0xFF, 0x00,
+ 0x07,0x00, 0x01, 0xFF, 0x00,
+ 0x08,0x00, 0x03, 0xFF, 0x00};
+
+static char TAB2[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Preparation prise
+ 0x10,0x50, 0x01, 0xFF, 0x03,
+ 0x07,0xF4, 0x01, 0xFF, 0x03,
+ 0x08,0xF4, 0x01, 0xFF, 0x03};
+
+static char TAB3[25]= {0x01,0x50, 0x00, 0xFF, 0x03, ///Stockage haut (pince fermee)
+ 0x10,0x50, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB4[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (pince en l'air)
+ 0x10,0x50, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB5[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige)
+ 0x10,0xF4, 0x01, 0xFF, 0x00,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB6[25]= {0x01,0xB0, 0x01, 0xFF, 0x03, ///Stockage haut (pince ouverte)
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0x00, 0x01, 0xFF, 0x03,
+ 0x08,0x00, 0x03, 0xFF, 0x03};
+
+static char TAB7[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage bas (pince en l'air)
+ 0x10,0xB0, 0x00, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x00,
+ 0x08,0x4D, 0x03, 0xFF, 0x00};
+
+static char TAB8[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Preparation_depot_bas
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x00,
+ 0x08,0x4D, 0x03, 0xFF, 0x00};
+
+static char TAB9[25]= {0x01,0x40, 0x00, 0xFF, 0x03, ///Deposer
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0xD0, 0x00, 0xFF, 0x00,
+ 0x08,0x35, 0x03, 0xFF, 0x00};
+
+static char TAB10[25]= {0x01,0xA0, 0x01, 0xFF, 0x03, ///Stockage haut (module sur tige)
+ 0x10,0x00, 0x01, 0xFF, 0x00,
+ 0x07,0xC5, 0x00, 0xFF, 0x03,
+ 0x08,0x4D, 0x03, 0xFF, 0x03};
+
+static char TAB11[25]= {0x01,0x60, 0x00, 0xFF, 0x03, ///Pousser_module
+ 0x10,0xF4, 0x01, 0xFF, 0x03,
+ 0x07,0xC5, 0x00, 0xFF, 0x00,
+ 0x08,0x4D, 0x03, 0xFF, 0x00};
+
+static char TAB12[25]= {0x01,0x05, 0x02, 0xFF, 0x03, ///Sortie position initiale
+ 0x10,0x00, 0x02, 0xFF, 0x03,
+ 0x07,0xF4, 0x01, 0xFF, 0x03,
+ 0x08,0xF4, 0x01, 0xFF, 0x03};
+
+static char TAB13[25]= {0x01,0xF4, 0x00, 0xFF, 0x03, ///Deposer
+ 0x10,0xA0, 0x02, 0xFF, 0x03,
+ 0x07,0xD0, 0x00, 0xFF, 0x00,
+ 0x08,0x35, 0x03, 0xFF, 0x00};
+
+////////////////////// TABLEAU BRAS GAUCHE ///////////////////////////
+static char TAB21[25]= {0x01,0x50, 0x03, 0xFF, 0x03, ///Position initiale
+ 0x02,0xF4, 0x01, 0xFF, 0x03};
+
+static char TAB22[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Preparation_tourner
+ 0x02,0x40, 0x03, 0xFF, 0x03};
+
+static char TAB23[25]= {0x01,0x20, 0x01, 0xFF, 0x03, ///Tourner_module
+ 0x02,0xE5, 0x02, 0xFF, 0x03};
+
+
+
+extern Timer t;
+
+typedef struct
+{
+ unsigned short Model_Number;
+ unsigned char Firmware;
+ unsigned char ID;
+ unsigned char Baud_Rate;
+ unsigned char Return_Delay_Time;
+ unsigned short CW_Angle_Limit;
+ unsigned short CCW_Angle_Limit;
+ unsigned char Reserved1;
+ unsigned char Highest_Limit_Temperature;
+ unsigned char Lowest_Limit_voltage;
+ unsigned char Highest_Limit_voltage;
+ unsigned short Max_Torque;
+ unsigned char Status_Return_Level;
+ unsigned char Alarm_LED;
+ unsigned char Alarm_Shutdown;
+ unsigned char Reserved2;
+ unsigned short Down_Calibration;
+ unsigned short Up_Calibration;
+ unsigned char Torque_Enable;
+ unsigned char LED;
+ unsigned char CW_Compliance_Margin;
+ unsigned char CCW_Compliance_Margin;
+ unsigned char CW_Compliance_Slope;
+ unsigned char CCW_Compliance_Slope;
+ unsigned short Goal_Position;
+ unsigned short Moving_Speed;
+ unsigned short Torque_Limit;
+ unsigned short Present_Position;
+ unsigned short Present_Speed;
+ unsigned short Present_Load;
+ unsigned char Present_Voltage;
+ unsigned char Present_Temperature;
+ unsigned char Registered_Instruction;
+ unsigned char Reserved3;
+ unsigned char Moving;
+ unsigned char Lock;
+ unsigned short Punch;
+} T_AX12;
+
+
+AX12::AX12(PinName tx, PinName rx, int ID, int baud)
+ : _ax12(tx,rx)
+{
+ _baud = baud;
+ _ID = ID;
+ _ax12.baud(_baud);
+
+
+}
+
+int AX12::Set_Secure_Goal(int degres)
+{
+ int error = 0;
+ int position = 0;
+ int difference = 0;
+ int timeout_secure = 0;
+ int autorisation = 0;
+
+ position = Get_Position();
+ error = Set_Goal(degres);
+
+ while ((autorisation == 0) && (timeout_secure < 100) )
+ {
+ position = Get_Position();
+ //printf("position : %d", position );
+ error = Set_Goal(degres);
+ //printf("degres : %d", degres);
+ difference = degres - position;
+ //printf ("difference : %d", difference );
+ if (((difference < 2) && (difference > (-2) )))
+ autorisation = 1;
+
+ timeout_secure++;
+ }
+
+ if ( timeout_secure == 100)
+ {
+ #ifdef AX12_DEBUG
+ printf (" timeout secure error ");
+ #endif
+ return(-1);
+ }
+ return(error);
+}
+
+
+int AX12::Get_Return_Delay_Time(void)
+{
+ char data[1];
+ int ErrorCode = read(_ID, AX12_REG_DELAY_TIME, 1, data);
+ int time = data[0];
+ time = 2.0 * time;
+ return(time);
+}
+
+
+int AX12::Get_Baud_Rate(void)
+{
+ char data[1];
+ int ErrorCode = read(_ID, AX12_REG_BAUD, 1, data);
+ int baud = data[0];
+ baud = 2000000 / ( baud + 1 );
+ return(baud);
+}
+
+
+/** Reglage du courant minimum necessaire au bon fonctionnement de l'actionneur
+// minimum >> Ox000 >> decimal 0
+// maximum >> 0x3FF >> decimal 1023
+// deflaut >> 0x20 >> decimal 32
+*/
+int AX12::Set_Punch(int punch)
+{
+ char data[2];
+
+ data[0] = punch & 0xff; // bottom 8 bits
+ data[1] = punch >> 8; // top 8 bits
+
+ // write the packet, return the error code
+ return (write(_ID, AX12_REG_PUNCH, 2, data));
+
+}
+
+/** Reset
+*/
+int AX12::Reset(int punch)
+{
+ char data[2];
+
+ data[0] = punch & 0xff; // bottom 8 bits
+ data[1] = punch >> 8; // top 8 bits
+
+ // write the packet, return the error code
+ return (write(_ID, 0x06, 1,data));
+
+}
+
+int AX12::Get_Punch (void)
+{
+ char data[2];
+ int ErrorCode = read(_ID, AX12_REG_PUNCH, 2, data);
+ int punch = data[0] | (data[1]<<8);
+ return(punch);
+}
+
+
+int AX12::Get_Load_Direction (void)
+{
+ char data[2];
+ int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data);
+ int direction = (data[1]>>2) & 0x01;
+ return(direction);
+}
+
+
+int AX12::Get_Load_Value (void)
+{
+ char data[2];
+ int ErrorCode = read(_ID, AX12_REG_PRESENT_LOAD, 2, data);
+ int Load = (data[0] | (data[1]<<8)) & 0x3FF;
+ return(Load);
+}
+
+
+int AX12::Get_Present_Speed (void)
+{
+ char data[2];
+ int ErrorCode = read(_ID, AX12_REG_PRESENT_SPEED, 2, data);
+ int speed = data[0] | (data[1]<<8);
+ return(speed);
+}
+
+
+int AX12::Get_CCW_Angle_Limit (void)
+{
+ char data[2];
+ int ErrorCode = read(_ID, AX12_REG_CCW_LIMIT, 2, data);
+ int angle = data[0] | (data[1]<<8);
+ angle = (angle * 300) / 1023;
+ return(angle);
+}
+
+
+int AX12::Get_CW_Angle_Limit (void)
+{
+ char data[2];
+ int ErrorCode = read(_ID, AX12_REG_CW_LIMIT, 2, data);
+ int angle = data[0] | (data[1]<<8);
+ angle = (angle * 300) / 1023;
+ return(angle);
+}
+
+
+
+int AX12::Get_Torque_Enable(void)
+{
+ char data[1];
+ int ErrorCode = read(_ID, AX12_REG_TORQUE_ENABLE, 1, data);
+ int enable = data[0];
+ return(enable);
+}
+
+
+int AX12::Set_Torque_Enable(int etat)
+{
+ char data[1];
+ data [0] = etat;
+
+ int error = write(_ID, AX12_REG_TORQUE_ENABLE, 1, data);
+ return (error);
+}
+
+
+
+int AX12::Get_Up_Calibration (void)
+{
+ char data[1];
+ int ErrorCode = read(_ID, AX12_REG_UP_CALIBRATION, 2, data);
+ int Up_calibration = data[0] | (data[1]<<8);
+ return(Up_calibration);
+}
+
+
+
+int AX12::Get_Down_Calibration (void)
+{
+ char data[1];
+ int ErrorCode = read(_ID, AX12_REG_DOWN_CALIBRATION, 2, data);
+ int Dowm_calibration = data[0] | (data[1]<<8);
+ return(Dowm_calibration);
+}
+
+
+
+int AX12::Get_ID(void)
+{
+
+ char data[1]={-1};
+ int ErrorCode = read(_ID, AX12_REG_ID, 1, data);
+ int id = data[0];
+ return(id);
+}
+
+
+// Lecture du couple maximum ( retourne la valeur du registre Max Torque de l'AX12 )
+int AX12::Get_Max_Torque (void)
+{
+ char data[2];
+ int ErrorCode = read(_ID, AX12_REG_MAX_TORQUE, 2, data);
+ int torque = data[0] | (data[1]<<8);
+ return(torque);
+}
+
+
+
+/** Reglage du couple maximum de l'actionneur
+// minimum >> Ox000 >> decimal 0
+// maximum >> 0x3FF >> decimal 1023
+// deflaut >> >> decimal
+*/
+int AX12::Set_Max_Torque(int torque)
+{
+ char data[2];
+
+ data[0] = torque & 0xff; // bottom 8 bits
+ data[1] = torque >> 8; // top 8 bits
+
+ // write the packet, return the error code
+ return (write(_ID, AX12_REG_MAX_TORQUE, 2, data));
+
+}
+
+
+
+
+/** Reglage de la desactivation des actionneurs si une erreur le concernant se produit
+Bit Function
+Bit 7 0
+Bit 6 If set to 1, torque off when an Instruction Error occurs
+Bit 5 If set to 1, torque off when an Overload Error occurs
+Bit 4 If set to 1, torque off when a Checksum Error occurs
+Bit 3 If set to 1, torque off when a Range Error occurs
+Bit 2 If set to 1, torque off when an Overheating Error occurs
+Bit 1 If set to 1, torque off when an Angle Limit Error occurs
+Bit 0 If set to 1, torque off when an Input Voltage Error occurs
+*/
+int AX12::Set_Alarm_Shutdown(int valeur)
+{
+ char data[1];
+ data [0] = valeur;
+
+ int val_alarm_shutdown = write(_ID, AX12_REG_ALARM_SHUTDOWN, 1, data);
+ return (val_alarm_shutdown);
+}
+
+
+
+/** Reglage de l'activation de l'alarme
+Bit Function
+Bit 7 0
+Bit 6 If set to 1, the LED blinks when an Instruction Error occurs
+Bit 5 If set to 1, the LED blinks when an Overload Error occurs
+Bit 4 If set to 1, the LED blinks when a Checksum Error occurs
+Bit 3 If set to 1, the LED blinks when a Range Error occurs
+Bit 2 If set to 1, the LED blinks when an Overheating Error occurs
+Bit 1 If set to 1, the LED blinks when an Angle Limit Error occurs
+Bit 0 If set to 1, the LED blinks when an Input Voltage Error occurs
+*/
+int AX12::Set_Alarm_LED(int valeur)
+{
+ char data[1];
+ data [0] = valeur;
+
+ int val_alarmLED = write(_ID, AX12_REG_ALARM_LED, 1, data);
+ return (val_alarmLED);
+}
+
+
+
+
+// Reglage de la réponse à une instruction
+// 0 >> ne repond a aucune instructions
+// 1 >> repond seulement aux instructions READ_DATA
+// 2 >> repond à toutes les instructions
+int AX12::Set_Status_Return_Level(int etat)
+{
+ char data[1];
+ data [0] = etat;
+
+ int val_return_lvl = write(_ID, AX12_REG_SATUS_RETURN, 1, data);
+ return (val_return_lvl);
+}
+
+
+// Reglage de la tension minimale
+// minimum >> Ox32 >> decimal 50
+// maximum >> 0xFA >> decimal 250
+// deflaut >> 0x3C >> decimal 60
+int AX12::Set_Lowest_Voltage(int val_lowest_voltage)
+{
+ char data[1];
+ data [0] = val_lowest_voltage;
+
+ int val_lowvolt = write(_ID, AX12_REG_LOWEST_VOLTAGE, 1, data);
+ return (val_lowvolt);
+}
+
+
+// Reglage de la tension maximale
+// minimum >> Ox32 >> decimal 50
+// maximum >> 0xFA >> decimal 250
+// deflaut >> 0xBE >> decimal 190
+int AX12::Set_Highest_Voltage(int val_highest_voltage)
+{
+ char data[1];
+ data [0] = val_highest_voltage;
+
+ int val_highvolt = write(_ID, AX12_REG_HIGHEST_VOLTAGE, 1, data);
+ return (val_highvolt);
+}
+
+
+// Reglage du return time delay EN MICRO SECONDE 2uSec * val_delay_time
+// minimum >> 0 us
+// maximum >> 508 us
+// deflaut >> 125 us
+int AX12::Set_Delay_Time (int val_delay_time )
+ {
+ char data[1];
+ data [0] = val_delay_time/2.0;
+
+ int valdelay_time = write(_ID, AX12_REG_DELAY_TIME, 1, data);
+ return (valdelay_time );
+ }
+
+
+// Reglage de la température max du cervo
+// minimum >> Ox00 >> decimal 0
+// maximum >> 0x96 >> decimal 150
+int AX12::Set_Temperature_Max (int val_temperature )
+ {
+ char data[1];
+ data [0] = val_temperature;
+
+ int valtemp_max = write(_ID, AX12_REG_TEMP_MAX, 1, data);
+ return (valtemp_max );
+ }
+
+// Etat LED
+// 0 = off
+// 1 = on
+int AX12::Set_Etat_LED(int etat)
+{
+ char data[1];
+ data [0] = etat;
+
+ int valLED = write(_ID, AX12_REG_LED, 1, data);
+ return (valLED);
+}
+
+// Set the mode of the servo
+// 0 = Positional (0-300 degrees)
+// 1 = Rotational -1 to 1 speed
+int AX12::Set_Mode(int mode)
+{
+
+ if (mode == 1) { // set CR
+ //wait(0.001);
+ Set_CW_Angle_Limit(0);
+ //wait(0.001);
+ Set_CCW_Angle_Limit(0);
+ //wait(0.001);
+ Set_CR_Speed(0.0);
+ //wait(0.001);
+ } else {
+ //wait(0.001);
+ Set_CW_Angle_Limit(0);
+ //wait(0.001);
+ Set_CCW_Angle_Limit(300);
+ //wait(0.001);
+ Set_CR_Speed(0.0);
+ //wait(0.001);
+ }
+ return(0);
+}
+
+int AX12::Set_Goal_speed(int speed, int flags)
+{
+
+ char reg_flag = 0;
+ char data[2];
+
+ // set the flag is only the register bit is set in the flag
+ if (flags == 0x2) {
+ reg_flag = 1;
+ }
+
+ data[0] = speed & 0xff; // bottom 8 bits
+ data[1] = speed >> 8; // top 8 bits
+
+ // write the packet, return the error code
+ int rVal = write(_ID, AX12_REG_MOVING_SPEED, 2, data, reg_flag);
+
+ /*if (flags == 1) {
+ // block until it comes to a halt
+ while (isMoving())
+ {
+ }
+
+ }*/
+ return(rVal);
+}
+
+
+// if flag[0] is set, were blocking
+// if flag[1] is set, we're registering
+// they are mutually exclusive operations
+int AX12::Set_Goal(int degrees, int flags)
+{
+
+ char reg_flag = 0;
+ char data[2];
+
+ // set the flag is only the register bit is set in the flag
+ if (flags == 0x2) {
+ reg_flag = 1;
+ }
+
+ // 1023 / 300 * degrees
+ short goal = (1023 * degrees) / 300;
+#ifdef AX12_DEBUG
+ printf("SetGoal to 0x%x\n",goal);
+#endif
+
+ data[0] = goal & 0xff; // bottom 8 bits
+ data[1] = goal >> 8; // top 8 bits
+
+ // write the packet, return the error code
+ int rVal = write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
+
+ /*if (flags == 1) {
+ // block until it comes to a halt
+ while (isMoving())
+ {
+ }
+
+ }*/
+ return(rVal);
+}
+
+
+// Set continuous rotation speed from -1 to 1
+int AX12::Set_CR_Speed(float speed)
+{
+
+ // bit 10 = direction, 0 = CCW, 1=CW
+ // bits 9-0 = Speed
+ char data[2];
+
+ int goal = (0x3ff * abs(speed));
+
+ // Set direction CW if we have a negative speed
+ if (speed < 0) {
+ goal |= (0x1 << 10);
+ }
+
+ data[0] = goal & 0xff; // bottom 8 bits
+ data[1] = goal >> 8; // top 8 bits
+
+ // write the packet, return the error code
+ int rVal = write(_ID, 0x20, 2, data);
+
+ return(rVal);
+}
+
+
+int AX12::Set_CW_Angle_Limit (int degrees)
+{
+
+ char data[2];
+
+ // 1023 / 300 * degrees
+ short limit = (1023 * degrees) / 300;
+
+#ifdef AX12_DEBUG
+ printf("SetCWLimit to 0x%x\n",limit);
+#endif
+
+ data[0] = limit & 0xff; // bottom 8 bits
+ data[1] = limit >> 8; // top 8 bits
+
+ // write the packet, return the error code
+ return (write(_ID, AX12_REG_CW_LIMIT, 2, data));
+
+}
+
+int AX12::Set_CCW_Angle_Limit (int degrees)
+{
+
+ char data[2];
+
+ // 1023 / 300 * degrees
+ short limit = (1023 * degrees) / 300;
+
+#ifdef AX12_DEBUG
+ printf("SetCCWLimit to 0x%x\n",limit);
+#endif
+
+ data[0] = limit & 0xff; // bottom 8 bits
+ data[1] = limit >> 8; // top 8 bits
+
+ // write the packet, return the error code
+ return (write(_ID, AX12_REG_CCW_LIMIT, 2, data));
+}
+
+
+int AX12::Set_ID (int CurrentID, int NewID)
+{
+
+ char data[1];
+ data[0] = NewID;
+
+#ifdef AX12_DEBUG
+ printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
+#endif
+
+ return (write(CurrentID, AX12_REG_ID, 1, data));
+
+}
+
+
+int AX12::Set_Baud (int baud)
+{
+
+ char data[1];
+ data[0] = baud;
+
+#ifdef AX12_DEBUG
+ printf("Setting Baud rate to %d\n",baud);
+#endif
+
+ return (write(_ID, AX12_REG_BAUD, 1, data));
+
+}
+
+
+
+// return 1 is the servo is still in flight
+int AX12::isMoving(void)
+{
+
+ char data[1];
+ read(_ID,AX12_REG_MOVING,1,data);
+ return(data[0]);
+}
+
+void AX12::reset()
+{
+
+ unsigned char TxBuf[16];
+ unsigned char sum = 0;
+ unsigned long debut=0;
+
+#ifdef AX12_TRIGGER_DEBUG
+ // Build the TxPacket first in RAM, then we'll send in one go
+ printf("\nreset\n");
+ printf("\nreset Packet\n Header : 0xFF, 0xFF\n");
+#endif
+
+ TxBuf[0] = 0xFF;
+ TxBuf[1] = 0xFF;
+
+ // ID - Broadcast
+ TxBuf[2] =_ID;
+ sum += TxBuf[2];
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" ID : %d\n",TxBuf[2]);
+#endif
+
+ // Length
+ TxBuf[3] = 0x02;
+ sum += TxBuf[3];
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" Length %d\n",TxBuf[3]);
+#endif
+
+ // Instruction - ACTION
+ TxBuf[4] = 0x06; //reset
+ sum += TxBuf[4];
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" Instruction 0x%X\n",TxBuf[5]);
+#endif
+
+ // Checksum
+ TxBuf[5] = 0xFF - sum;
+//#ifdef AX12_TRIGGER_DEBUG
+ printf(" Checksum 0x%X\n",TxBuf[5]);
+//#endif
+
+ // Transmit the packet in one burst with no pausing
+ for (int i = 0; i < 6 ; i++)
+ {
+ while(_ax12.writeable()==0);
+ _ax12.putc(TxBuf[i]);
+
+ }
+ wait(0.001);
+ debut=t.read_ms();
+
+ do
+ {
+ if (_ax12.readable()==-1) // reception du premier Header ( 0xFF )
+ printf("%02x",_ax12.getc());
+ }
+ while((t.read_ms()-debut)<500);
+
+ printf("\n");
+ return;
+}
+
+void AX12::read_all_info(unsigned char start, unsigned char longueur)
+{
+
+ unsigned char TxBuf[16];
+ unsigned char sum = 0;
+ unsigned long debut=0;
+
+#ifdef AX12_TRIGGER_DEBUG
+ // Build the TxPacket first in RAM, then we'll send in one go
+ printf("\nreset\n");
+ printf("\nreset Packet\n Header : 0xFF, 0xFF\n");
+#endif
+
+ TxBuf[0] = 0xFF;
+ TxBuf[1] = 0xFF;
+
+ // ID - Broadcast
+ TxBuf[2] =_ID;
+ sum += TxBuf[2];
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" ID : %d\n",TxBuf[2]);
+#endif
+
+ // Length
+ TxBuf[3] = 0x04;
+ sum += TxBuf[3];
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" Length %d\n",TxBuf[3]);
+#endif
+
+ // Instruction - ACTION
+ TxBuf[4] = INST_READ; //reset
+ sum += TxBuf[4];
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" Instruction 0x%X\n",TxBuf[4]);
+#endif
+
+ TxBuf[5] = start; //reset
+ sum += TxBuf[5];
+
+ TxBuf[6] = longueur; //reset
+ sum += TxBuf[6];
+
+
+ // Checksum
+ TxBuf[7] = 0xFF - sum;
+//#ifdef AX12_TRIGGER_DEBUG
+ //printf(" Checksum 0x%X\n\r",TxBuf[7]);
+//#endif
+
+ // Transmit the packet in one burst with no pausing
+ for (int i = 0; i < 8 ; i++)
+ {
+ while(_ax12.writeable()==0);
+ _ax12.putc(TxBuf[i]);
+
+ }
+
+ debut=t.read_ms();
+ int i=0;
+ do
+ {
+ if (_ax12.readable())
+ { // reception du premier Header ( 0xFF )
+ printf("%02d:%02x ",start+i,_ax12.getc());
+ i++;
+ }
+ }
+ while((t.read_ms()-debut)<5000);
+
+ printf("\n");
+ return;
+}
+
+
+void AX12::trigger(void)
+{
+
+ char TxBuf[16];
+ char sum = 0;
+
+#ifdef AX12_TRIGGER_DEBUG
+ // Build the TxPacket first in RAM, then we'll send in one go
+ printf("\nTriggered\n");
+ printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
+#endif
+
+ TxBuf[0] = 0xFF;
+ TxBuf[1] = 0xFF;
+
+ // ID - Broadcast
+ TxBuf[2] = 0xFE;
+ sum += TxBuf[2];
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" ID : %d\n",TxBuf[2]);
+#endif
+
+ // Length
+ TxBuf[3] = 0x02;
+ sum += TxBuf[3];
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" Length %d\n",TxBuf[3]);
+#endif
+
+ // Instruction - ACTION
+ TxBuf[4] = 0x04;
+ sum += TxBuf[4];
+
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" Instruction 0x%X\n",TxBuf[5]);
+#endif
+
+ // Checksum
+ TxBuf[5] = 0xFF - sum;
+#ifdef AX12_TRIGGER_DEBUG
+ printf(" Checksum 0x%X\n",TxBuf[5]);
+#endif
+
+ // Transmit the packet in one burst with no pausing
+ for (int i = 0; i < 6 ; i++) {
+ _ax12.putc(TxBuf[i]);
+ }
+
+ // This is a broadcast packet, so there will be no reply
+ return;
+}
+
+
+float AX12::Get_Position(void)
+{
+
+ #ifdef AX12_DEBUG
+ printf("\nGetPositionID(%d)",_ID);
+ #endif
+
+ char data[2];
+
+ int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
+ int position = data[0] | (data[1] << 8);
+ float angle = ((float)position * 300.0)/1023.0;
+
+ return (angle);
+}
+
+
+float AX12::Get_Temp ()
+{
+
+#ifdef AX12_DEBUG
+ printf("\nGetTemp(%d)",_ID);
+#endif
+
+ char data[1];
+ int ErrorCode = read(_ID, AX12_REG_TEMP, 1, data);
+ float temp = data[0];
+ return(temp);
+}
+
+
+float AX12::Get_Volts (void)
+{
+
+#ifdef AX12_DEBUG
+ printf("\nGetVolts(%d)",_ID);
+#endif
+
+ char data[1];
+ int ErrorCode = read(_ID, AX12_REG_VOLTS, 1, data);
+ float volts = data[0]/10.0;
+ return(volts);
+}
+
+
+int AX12::read(int ID, int start, int bytes, char* data) {
+
+
+ char PacketLength = 0x3;
+ char TxBuf[16];
+ char sum = 0;
+ char Status[16];
+
+ int timeout = 0;
+ int plen = 0;
+ int flag_out = 0;
+ int timeout_transmit = 0;
+ int i = 0;
+ int enable = 0;
+// int poubelle = 0;
+// int count = 0;
+// char vidage[50];
+
+ typedef enum {Header1, Header2, ident, length, erreur, reception, checksum} type_etat;
+ type_etat etat = Header1;
+
+ Status[4] = 0xFE; // return code
+
+
+
+
+
+/*********************************** CREATION DE LA TRAME A EVOYER *****************************************/
+
+#ifdef AX12_READ_DEBUG
+ printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
+#endif
+
+ // Build the TxPacket first in RAM, then we'll send in one go
+#ifdef AX12_READ_DEBUG
+ printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
+#endif
+
+ TxBuf[0] = 0xff;
+ TxBuf[1] = 0xff;
+
+ // ID
+ TxBuf[2] = ID;
+ sum += TxBuf[2];
+
+#ifdef AX12_READ_DEBUG
+ printf(" ID : %d\n",TxBuf[2]);
+#endif
+
+ // Packet Length
+ TxBuf[3] = 4;//PacketLength+bytes; // Length = 4 ; 2 + 1 (start) = 1 (bytes)
+ sum += TxBuf[3]; // Accululate the packet sum
+
+#ifdef AX12_READ_DEBUG
+ printf(" Length : 0x%x\n",TxBuf[3]);
+#endif
+
+ // Instruction - Read
+ TxBuf[4] = 0x2;
+ sum += TxBuf[4];
+
+#ifdef AX12_READ_DEBUG
+ printf(" Instruction : 0x%x\n",TxBuf[4]);
+#endif
+
+ // Start Address
+ TxBuf[5] = start;
+ sum += TxBuf[5];
+
+#ifdef AX12_READ_DEBUG
+ printf(" Start Address : 0x%x\n",TxBuf[5]);
+#endif
+
+ // Bytes to read
+ TxBuf[6] = bytes;
+ sum += TxBuf[6];
+
+#ifdef AX12_READ_DEBUG
+ printf(" No bytes : 0x%x\n",TxBuf[6]);
+#endif
+
+ // Checksum
+ TxBuf[7] = 0xFF - sum;
+#ifdef AX12_READ_DEBUG
+ printf(" Checksum : 0x%x\n",TxBuf[7]);
+#endif
+/********************************************TRAME CONSTRUITE DANS TxBuf***************************************/
+
+
+
+
+ /* Transmission de la trame construite precedemment dans le tableau TxBuf
+ */
+ while ((timeout_transmit<5000) && (i < (7+bytes)))
+ {
+ if (_ax12.writeable())
+ {
+ _ax12.putc(TxBuf[i]);
+ i++;
+ timeout_transmit = 0;
+ }
+ else timeout_transmit++;
+ }
+
+ if (timeout_transmit == 5000 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
+ {
+ #ifdef AX12_DEBUG
+ printf ("timeout transmit erreur\r\n");
+ #endif
+ return(-1);
+ }
+ /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
+ */
+
+
+ // Wait for the bytes to be transmitted
+ wait (0.001);
+
+
+
+ // Skip if the read was to the broadcast address
+ if (_ID != 0xFE) {
+
+
+
+ /* Partie de reception de la trame de retour
+ */
+ while ((flag_out != 1) && (timeout < (1000*bytes)))
+ {
+ // Les differents etats de l'automate on été créés au debut de la fonction write !
+ switch (etat)
+ {
+ case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF )
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ if (Status[plen] == 0xFF )
+ {
+ etat = Header2;
+ #ifdef AX12_DEBUG_READ
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+
+ }
+ else etat = Header1;
+ }
+ else timeout++;
+ break;
+
+
+ case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF )
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ if (Status[plen] == 0xFF )
+ {
+ etat = ident;
+ #ifdef AX12_DEBUG_READ
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+
+ }
+ else if (Status[plen] == ID ) // PERMET D'EVITER CERTAINES ERREUR LORSQU'ON LIT PLUSIEURS REGISTRES !!!!
+ {
+ Status[plen] = 0;
+ #ifdef AX12_DEBUG_READ
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+ Status[plen] = ID;
+ etat = length;
+ #ifdef AX12_DEBUG_READ
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+
+ }
+ else
+ {
+
+ etat = Header1;
+ plen = 0;
+ }
+ }
+ else timeout++;
+ break;
+
+ case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ if (Status[plen] == ID )
+ {
+ etat = length;
+ #ifdef AX12_DEBUG_READ
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+
+ }
+ else
+ {
+ etat = Header1;
+ plen = 0;
+ }
+ }
+ else timeout++;
+ break;
+
+ case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ if (Status[plen] == (bytes+2) )
+ {
+ etat = erreur;
+ #ifdef AX12_DEBUG_READ
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+
+ }
+ else
+ {
+ etat = Header1;
+ plen = 0;
+ }
+ }
+ else timeout++;
+ break;
+
+ case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ #ifdef AX12_DEBUG_READ
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+
+ etat = reception;
+ }
+ else timeout++;
+
+ case reception: while ( enable < bytes ) // reception du ou des octect(s) de donnés ( suivant la valeur de la variable length )
+ {
+ if (_ax12.readable())
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ #ifdef AX12_DEBUG_READ
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+ enable++;
+
+ }
+ else timeout++;
+ }
+ etat = checksum;
+ break;
+
+ case checksum: if (_ax12.readable()) // reception du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length + somme des données ) >>>> dans le cas d'un retour d'un read!!
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ flag_out = 1;
+ etat = Header1;
+
+ #ifdef AX12_DEBUG_READ
+ printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]);
+ #endif
+ }
+ else timeout++;
+ break;
+
+ default: break;
+ }
+ }
+
+
+ if (timeout == (1000*bytes) ) // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames
+ {
+ #ifdef AX12_DEBUG
+ printf ("timeout erreur\n");
+ #endif
+ return(-1);
+ }
+
+
+ // copie des données dans le tableau data
+ for (int i=0; i < Status[3]-2 ; i++)
+ {
+ data[i] = Status[5+i];
+ }
+
+ } // toute la partie precedente ne s'effectue pas dans le cas d'un appel avec un broadcast ID (ID!=0xFE)
+
+ return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
+}
+
+void AX12::multiple_goal_and_speed(int number_ax12,char* tab)
+{
+ char TxBuf[50];
+ char sum = 0;
+ int timeout_transmit =0;
+ int i=0, k=0, j=0;
+ int L=4; // nombre instructions par paquets
+ int bytes= ((L+1)*number_ax12)+4;
+
+ typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
+ type_etat etat= Header1;
+
+ for(j=0; j<50; j++)
+ {
+ TxBuf[i]=0;
+ }
+
+ #ifdef AX12_WRITE_DEBUG
+ //printf(" MULTIPLE_GOAL_AND_SPEED \n ");
+ #endif
+
+// Build the TxPacket first in RAM, then we'll send in one go
+ #ifdef AX12_WRITE_DEBUG
+ //printf("\nInstruction Packet\n Header :%d, %d\n",TxBuf[0], TxBuf[1]);
+ #endif
+
+ TxBuf[0]=0xFF; // bit de start
+ TxBuf[1]=0xFF; // bit de start
+
+ TxBuf[2] = 0xFE; //ID broadcast
+ sum += TxBuf[2];
+
+ #ifdef AX12_WRITE_DEBUG
+ printf(" adresse de difusion : %d\n",TxBuf[2]);
+ #endif
+
+ TxBuf[3] =bytes; // longueur
+ sum += TxBuf[3];
+
+ #ifdef AX12_WRITE_DEBUG
+ printf(" Longueur : %d\n",TxBuf[3]);
+ #endif
+
+ TxBuf[4]=0x83; //SYNC_WRITE
+ sum += TxBuf[4];
+
+ #ifdef AX12_WRITE_DEBUG
+ printf(" Instruction : 0x%x\n",TxBuf[4]);
+ #endif
+
+ TxBuf[5] = 0x1E; // addresse "GOAL_POSITION"
+ sum += TxBuf[5];
+
+ #ifdef AX12_WRITE_DEBUG
+ printf(" Adresse de debut : 0x%x\n",TxBuf[5]);
+ #endif
+
+ TxBuf[6]=L; // Nombre instruction par paquets
+ sum += TxBuf[6];
+
+ #ifdef AX12_WRITE_DEBUG
+ printf(" nombre instruction/paquet : 0x%x\n",TxBuf[6]);
+ #endif
+
+ for(i=0; i<(number_ax12*5); i++) // Copie des data de TAB sur TxBuf
+ {
+
+ TxBuf[i+7]=tab[i];
+ sum += TxBuf[i+7];
+
+ }
+
+ #ifdef AX12_WRITE_DEBUG
+ for(i=0; i<(number_ax12*5); i++)
+ {
+
+ printf(" Data : 0x%x\n",TxBuf[i+7]);
+
+ }
+ #endif
+
+ TxBuf[(number_ax12*5)+7] = 0xFF - sum ; // CHECKSUM
+
+ #ifdef AX12_WRITE_DEBUG
+ printf(" Checksum : 0x%x\n",TxBuf[(number_ax12*5)+9]);
+ #endif
+
+ for(k=0; k<((number_ax12*5)+8); k++) // TRANSMISSION DE LA TRAME
+ {
+ _ax12.putc(TxBuf[k]);
+ #ifdef AX12_WRITE_DEBUG
+ printf(" transmission : 0x%x\n",TxBuf[k]);
+ #endif
+ }
+
+
+}
+
+float AX12::read_and_test(float angle,char* Tab)
+{
+ int k=0;
+ unsigned short val_angle=0, val_reche=0;
+
+ #ifdef AX12_DEBUG
+ printf("\nread_and_test");
+ #endif
+
+ if( _ID==0x12)
+ { k=1;}
+ else if( _ID==0x04)
+ { k=6;}
+ else if( _ID==0x07)
+ { k=11;}
+ else if( _ID==0x0F)
+ { k=16;}
+
+ val_angle = (unsigned short) (angle/0.3);
+ val_reche = (unsigned short) Tab[k] + ((unsigned short)Tab[k+1]<<8);
+
+ if((val_angle < (val_reche+(28))) && (val_angle > (val_reche-(28))))
+ {
+ #ifdef AX12_DEBUG
+ printf("\nreturn1");
+ #endif
+ return 1;
+ }
+ else
+ {
+ #ifdef AX12_DEBUG
+ printf("\nreturn0");
+ #endif
+ return 0;
+ }
+
+}
+
+int AX12::write(int ID, int start, int bytes, char* data, int flag)
+{
+// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
+
+ char TxBuf[16];
+ char sum = 0;
+ char Status[6];
+
+ int timeout = 0;
+ int plen = 0;
+ int flag_out = 0;
+ int timeout_transmit = 0;
+ int i = 0;
+ int poubelle = 0;
+ int count = 0;
+ char vidage[50];
+
+ typedef enum {Header1, Header2, ident, length, erreur, checksum} type_etat;
+ type_etat etat = Header1;
+
+#ifdef AX12_WRITE_DEBUG
+ printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
+#endif
+
+ // Build the TxPacket first in RAM, then we'll send in one go
+#ifdef AX12_WRITE_DEBUG
+ printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n");
+#endif
+
+ TxBuf[0] = 0xff;
+ TxBuf[1] = 0xff;
+
+ // ID
+ TxBuf[2] = ID;
+ sum += TxBuf[2];
+
+#ifdef AX12_WRITE_DEBUG
+ printf(" ID : %d\n",TxBuf[2]);
+#endif
+
+ // packet Length
+ TxBuf[3] = 3+bytes;
+ sum += TxBuf[3];
+
+#ifdef AX12_WRITE_DEBUG
+ printf(" Length : %d\n",TxBuf[3]);
+#endif
+
+ // Instruction
+ if (flag == 1) {
+ TxBuf[4]=0x04;
+ sum += TxBuf[4];
+ } else {
+ TxBuf[4]=0x03;
+ sum += TxBuf[4];
+ }
+
+#ifdef AX12_WRITE_DEBUG
+ printf(" Instruction : 0x%x\n",TxBuf[4]);
+#endif
+
+ // Start Address
+ TxBuf[5] = start;
+ sum += TxBuf[5];
+
+#ifdef AX12_WRITE_DEBUG
+ printf(" Start : 0x%x\n",TxBuf[5]);
+#endif
+
+ // data
+ for (char i=0; i<bytes ; i++) {
+ TxBuf[6+i] = data[i];
+ sum += TxBuf[6+i];
+
+#ifdef AX12_WRITE_DEBUG
+ printf(" Data : 0x%x\n",TxBuf[6+i]);
+#endif
+
+ }
+
+ // checksum
+ TxBuf[6+bytes] = 0xFF - sum;
+
+#ifdef AX12_WRITE_DEBUG
+ printf(" Checksum : 0x%x\n",TxBuf[6+bytes]);
+#endif
+
+
+ /* Transmission de la trame construite precedemment dans le tableau TxBuf
+ */
+ while ((timeout_transmit<100) && (i < (7+bytes)))
+ {
+ if (_ax12.writeable())
+ {
+ _ax12.putc(TxBuf[i]);
+ i++;
+ timeout_transmit = 0;
+ }
+ else timeout_transmit++;
+ }
+
+ if (timeout_transmit == 100 ) // dans le cas d'une sortie en timeout pour ne pas rester bloquer !
+ {
+ #ifdef AX12_DEBUG
+ printf ("TIMEOUT TRANSMIT ERROR\r\n");
+ #endif
+ return(-1);
+ }
+ /* Transmission effectuée on va ensuite récuperer la trame de retour renvoyer par le servomoteur
+ */
+
+
+ // Wait for data to transmit
+ wait (0.005);
+
+ // make sure we have a valid return
+ Status[4]=0x00;
+
+ // we'll only get a reply if it was not broadcast
+ if (_ID!=0xFE) {
+
+
+ /* Partie de reception de la trame de retour
+ */
+ while ((flag_out != 1) && (timeout < MAX_TIMEOUT))
+ {
+ // Les differents etats de l'automate on été créés au debut de la fonction write !
+ switch (etat)
+ {
+ case Header1: if (_ax12.readable()) // reception du premier Header ( 0xFF )
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ if (Status[plen] == 0xFF )
+ {
+ etat = Header2;
+ #ifdef AX12_DEBUG_WRITE
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+
+ }
+ else etat = Header1;
+ }
+ else timeout++;
+ break;
+
+
+ case Header2: if (_ax12.readable()) // reception du second Header ( 0xFF )
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ if (Status[plen] == 0xFF )
+ {
+ etat = ident;
+ #ifdef AX12_DEBUG_WRITE
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+ }
+ else
+ {
+ etat = Header1;
+ plen = 0;
+ }
+ }
+ else timeout++;
+ break;
+
+ case ident: if (_ax12.readable()) // reception de l'octet correspondant à l'ID du servomoteur
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ if (Status[plen] == ID )
+ {
+ etat = length;
+ #ifdef AX12_DEBUG_WRITE
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+ }
+ else
+ {
+ etat = Header1;
+ plen = 0;
+ }
+ }
+ else timeout++;
+ break;
+
+ case length: if (_ax12.readable()) // reception de l'octet correspondant à la taille ( taille = 2 + nombre de paramètres )
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ if (Status[plen] == 2 ) // dans la trame de retour d'un write il n'y a pas de paramètre la taille vaudra donc 2!!
+ {
+ etat = erreur;
+ #ifdef AX12_DEBUG_WRITE
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+ }
+ else
+ {
+ etat = Header1;
+ plen = 0;
+ }
+ }
+ else timeout++;
+ break;
+
+ case erreur: if (_ax12.readable()) //reception de l'octet correspondant au code d'erreurs eventuels ( 0 = pas d'erreur )
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ #ifdef AX12_DEBUG_WRITE
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ plen++;
+ etat = checksum;
+ }
+ else timeout++;
+
+ case checksum: if (_ax12.readable()) // recpetion du dernier octet ( Checksum ) >>> checksum = NOT ( ID + length ) >>>> dans le cas de la reception d'un write
+ {
+ Status[plen] = _ax12.getc();
+ timeout = 0;
+ flag_out = 1;
+ etat = Header1;
+ #ifdef AX12_DEBUG_WRITE
+ printf("data[%d] : %d\r\n\n", plen, (int)Status[plen]);
+ #endif
+ }
+ else timeout++;
+ break;
+ }
+ }
+
+
+ if ( Status[4] != 0 )
+ {
+ #ifdef AX12_DEBUG
+ printf ("erreur ! \r\n");
+ #endif
+ for (int i = 0; i<5; i++)
+ {
+ #ifdef AX12_DEBUG
+ printf("data[%d] : %d\r\n", plen, (int)Status[plen]);
+ #endif
+ }
+ }
+
+ if (timeout == MAX_TIMEOUT ) // permet d'afficher si il y a une erreur de timeout et de ne pas rester bloquer si il y a des erreurs de trames
+ {
+ #ifdef AX12_DEBUG
+ printf ("timeout erreur\n\r");
+ #endif
+ return(-1);
+ }
+
+ // Build the TxPacket first in RAM, then we'll send in one go
+ }
+
+ return(Status[4]); // retourne le code d'erreur ( octect 5 de la trame de retour )
+}
+
+
+
+
+//////////////////////////////////////////////////////////////////////////////
+
+/****************************************************************************************/
+/* FUNCTION NAME: Initialisation_position */
+/* DESCRIPTION : Fonction qui place les bras en position verticale */
+/****************************************************************************************/
+void Initialisation_position(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB12);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB1);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_prise */
+/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
+/****************************************************************************************/
+void Preparation_prise(void){
+ if (action_precedente == 0){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB12);
+ wait(TIME);
+ action_precedente = 1;
+ }
+ multiple_myAX12->multiple_goal_and_speed(4,TAB2);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_haut */
+/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
+/****************************************************************************************/
+void Stockage_haut(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB3);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB4);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB5);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB6);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_bas */
+/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
+/****************************************************************************************/
+void Stockage_bas(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB3);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB7);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Deposer */
+/* DESCRIPTION : Fonction qui permet de déposer le module */
+/****************************************************************************************/
+void Deposer(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB9);
+ wait(TIME/5);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB8);
+ wait(TIME/5);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB13);
+ wait(TIME/5);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_depot_bas */
+/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
+/****************************************************************************************/
+void Preparation_depot_bas(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB8);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_depot_haut */
+/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
+/****************************************************************************************/
+void Preparation_depot_haut(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB6);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB5);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB10);
+ wait(TIME);
+ multiple_myAX12->multiple_goal_and_speed(4,TAB8);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Pousser_module */
+/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
+/****************************************************************************************/
+void Pousser_module(void){
+ multiple_myAX12->multiple_goal_and_speed(4,TAB11);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Initialisation_gauche */
+/* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */
+/****************************************************************************************/
+void Initialisation_gauche(void){
+ trois_myAX12_2->Set_Secure_Goal(235);
+ multiple_myAX12_2->multiple_goal_and_speed(4,TAB22);
+ wait(TIME);
+ multiple_myAX12_2->multiple_goal_and_speed(4,TAB21);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_prise_gauche */
+/* DESCRIPTION : Fonction qui permet de preparer la recuperation d'un module */
+/****************************************************************************************/
+void Preparation_prise_gauche(void){
+ trois_myAX12_2->Set_Secure_Goal(120);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Prendre_module_gauche */
+/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */
+/****************************************************************************************/
+void Prendre_module_gauche(void){
+ trois_myAX12_2->Set_Secure_Goal(160);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_module_gauche */
+/* DESCRIPTION : Fonction qui prepare le tournante */
+/****************************************************************************************/
+void Preparation_module_gauche(void){
+ multiple_myAX12_2->multiple_goal_and_speed(4,TAB22);
+ wait(TIME);
+}
+
+/****************************************************************************************/
+/* FUNCTION NAME: Tourner_module_gauche */
+/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
+/****************************************************************************************/
+void Tourner_module_gauche(void){
+ multiple_myAX12_2->multiple_goal_and_speed(4,TAB23);
+ wait(TIME);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.h Thu May 11 12:55:52 2017 +0000
@@ -0,0 +1,542 @@
+/* mbed AX-12+ Servo Library
+ *
+ * Copyright (c) 2010, cstyles (http://mbed.org)
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#ifndef MBED_AX12_H
+#define MBED_AX12_H
+#include "SerialHalfDuplex.h"
+#include "global.h"
+
+//#define AX12_WRITE_DEBUG 0
+#define AX12_READ_DEBUG
+//#define AX12_TRIGGER_DEBUG 0
+#define AX12_DEBUG 0
+
+/****** à utiliser pour le debug !! ******/
+
+//#define AX12_DEBUG_WRITE 0
+//#define AX12_DEBUG_READ 0
+
+/******************************************/
+
+
+#define AX12_REG_ID 0x03
+#define AX12_REG_BAUD 0x04
+#define AX12_REG_DELAY_TIME 0x05
+#define AX12_REG_CW_LIMIT 0x06
+#define AX12_REG_CCW_LIMIT 0x08
+#define AX12_REG_TEMP_MAX 0x0B
+#define AX12_REG_LOWEST_VOLTAGE 0x0C
+#define AX12_REG_HIGHEST_VOLTAGE 0x0D
+#define AX12_REG_MAX_TORQUE 0x0E
+#define AX12_REG_SATUS_RETURN 0x10
+#define AX12_REG_ALARM_LED 0x11
+#define AX12_REG_ALARM_SHUTDOWN 0x12
+#define AX12_REG_DOWN_CALIBRATION 0x14
+#define AX12_REG_UP_CALIBRATION 0x16
+#define AX12_REG_TORQUE_ENABLE 0x18
+#define AX12_REG_LED 0x19
+#define AX12_REG_CW_MARGIN 0x1A
+#define AX12_REG_CCW_MARGIN 0x1B
+#define AX12_REG_CW_SLOPE 0x1C
+#define AX12_REG_CCW_SLOPE 0x1D
+#define AX12_REG_GOAL_POSITION 0x1E
+#define AX12_REG_MOVING_SPEED 0x20
+#define AX12_REG_TORQUE_LIMIT 0x22
+#define AX12_REG_POSITION 0x24
+#define AX12_REG_PRESENT_SPEED 0x26
+#define AX12_REG_PRESENT_LOAD 0x28
+#define AX12_REG_VOLTS 0x2A
+#define AX12_REG_TEMP 0x2B
+#define AX12_REG_INSTRUCTION 0x2C
+#define AX12_REG_MOVING 0x2E
+#define AX12_REG_LOCK 0x2F
+#define AX12_REG_PUNCH 0x30
+
+
+#define AX12_MODE_POSITION 0
+#define AX12_MODE_ROTATION 1
+
+#define AX12_CW 1
+#define AX12_CCW 0
+
+//--- Instruction ---
+#define INST_PING 0x01
+#define INST_READ 0x02
+#define INST_WRITE 0x03
+#define INST_REG_WRITE 0x04
+#define INST_ACTION 0x05
+#define INST_RESET 0x06
+#define INST_DIGITAL_RESET 0x07
+#define INST_SYSTEM_READ 0x0C
+#define INST_SYSTEM_WRITE 0x0D
+#define INST_SYNC_WRITE 0x83
+#define INST_SYNC_REG_WRITE 0x84
+
+#define DEFAULT_RETURN_PACKET_SIZE 6
+
+#define BROADCASTING_ID 0xfe
+
+/** Servo control class, based on a PwmOut
+ *
+ * Example:
+ * @code
+ * #include "mbed.h"
+ * #include "AX12.h"
+ *
+ * int main() {
+ *
+ * AX12 myax12 (p9, p10, 1);
+ *
+ * while (1) {
+ * myax12.SetGoal(0); // go to 0 degrees
+ * wait (2.0);
+ * myax12.SetGoal(300); // go to 300 degrees
+ * wait (2.0);
+ * }
+ * }
+ * @endcode
+ */
+class AX12
+{
+
+public:
+
+ /** Create an AX12 servo object connected to the specified serial port, with the specified ID
+ *
+ * @param pin tx pin
+ * @param pin rx pin
+ * @param int ID, the Bus ID of the servo 1-255
+ */
+ AX12(PinName tx, PinName rx, int ID, int baud=1000000);
+
+ /** Nouvelle fonction de commande de position du servomoteur avec sécurité d'arriver à la bonne position
+ Si une erreur se produit et que le servo ne recoit vraiment rien cette fonction dispose d'une sortie en timeout error;
+ */
+ int Set_Secure_Goal(int degres);
+
+ /** Retourne le temps d'attente avant l'envoi de la trame de retour par l'actionneur ( en micro seconde )
+ */
+ int Get_Return_Delay_Time(void);
+
+
+ /** Retourne la vitesse de communication de l'actionneur ( en Bits/seconde )
+ */
+ int Get_Baud_Rate(void);
+
+
+ /** Reglage du courant minimum necessaire au bon fonctionnement de l'actionneur
+ // minimum >> Ox000 >> decimal 0
+ // maximum >> 0x3FF >> decimal 1023
+ // deflaut >> 0x20 >> decimal 32
+ */
+ int Set_Punch(int punch);
+
+
+ /** Retourne le courant minimum au fonctionnement de l'actionneur
+ */
+ int Get_Punch (void);
+
+
+ /** Retourne l'ampleur de la charge sur l'actionneur
+ */
+ int Get_Load_Value (void);
+
+ void read_all_info(unsigned char, unsigned char);
+
+ /** Reset
+ */
+ int Reset(int);
+
+ /** Retourne la direction de la charge sur l'actionneur
+ */
+ int Get_Load_Direction (void);
+
+
+ /** Retourne la vitesse angulaire actuelle de l'actionneur
+ */
+ int Get_Present_Speed (void);
+
+
+ /** Retourne la valeur en degres du registre CCW angle limit qui est l'angle limite maximum de l'actionneur
+ */
+ int Get_CCW_Angle_Limit (void);
+
+
+ /** Retourne la valeur en degres du registre CW angle limit qui est l'angle limite minimum de l'actionneur
+ */
+ int Get_CW_Angle_Limit (void);
+
+
+ /** Retourne la valeur du registre Torque Enable
+ */
+ int Get_Torque_Enable(void);
+
+
+ /**
+ 1 >>>
+ 0 >>>
+ */
+ int Set_Torque_Enable(int etat);
+
+
+ /** Retourne les données de compensation des différences entre les potentiomètres
+ utilisés dans l'AX12 (Up) ????????
+ */
+ int Get_Up_Calibration (void);
+
+
+ /** Retourne les données de compensation des différences entre les potentiomètres
+ utilisés dans l'AX12 (Dowm) ????????
+ */
+ int Get_Down_Calibration(void);
+
+
+ /** Retourne l'ID de l'AX_12 avec lequel on dialogue
+ utile seulement dans le cas d'un broadcast ID
+ */
+ int Get_ID(void);
+
+
+ /** Reglage du couple maximum de l'actionneur
+ // minimum >> Ox000 >> decimal 0
+ // maximum >> 0x3FF >> decimal 1023
+ // deflaut >> >> decimal
+ */
+ int Set_Max_Torque(int torque);
+
+
+ /** Reglage de la desactivation des actionneurs si une erreur le concernant se produit
+ Bit Function
+ Bit 7 0
+ Bit 6 If set to 1, torque off when an Instruction Error occurs
+ Bit 5 If set to 1, torque off when an Overload Error occurs
+ Bit 4 If set to 1, torque off when a Checksum Error occurs
+ Bit 3 If set to 1, torque off when a Range Error occurs
+ Bit 2 If set to 1, torque off when an Overheating Error occurs
+ Bit 1 If set to 1, torque off when an Angle Limit Error occurs
+ Bit 0 If set to 1, torque off when an Input Voltage Error occurs
+ */
+ int Set_Alarm_Shutdown(int valeur);
+
+
+ /** Reglage de l'activation de l'alarme
+ Bit Function
+ Bit 7 0
+ Bit 6 If set to 1, the LED blinks when an Instruction Error occurs
+ Bit 5 If set to 1, the LED blinks when an Overload Error occurs
+ Bit 4 If set to 1, the LED blinks when a Checksum Error occurs
+ Bit 3 If set to 1, the LED blinks when a Range Error occurs
+ Bit 2 If set to 1, the LED blinks when an Overheating Error occurs
+ Bit 1 If set to 1, the LED blinks when an Angle Limit Error occurs
+ Bit 0 If set to 1, the LED blinks when an Input Voltage Error occurs
+ */
+ int Set_Alarm_LED(int valeur);
+
+
+ /** Reglage de la réponse à une instruction
+ * @param mode
+ * 0 >> ne repond a aucune instructions
+ * 1 >> repond seulement aux instructions READ_DATA
+ * 2 >> repond à toutes les instructions
+ */
+ int Set_Status_Return_Level(int etat);
+
+
+ /** Reglage de la tension minimale
+ * @param mode
+ * minimum >> 0x32 >> decimal 50
+ * maximum >> 0xFA >> decimal 250
+ * deflaut >> 0x3C >> decimal 60
+ */
+ int Set_Lowest_Voltage(int val_lowest_voltage);
+
+
+ /** Reglage de la tension maximale
+ * @param mode
+ * minimum >> Ox32 >> decimal 50
+ * maximum >> 0xFA >> decimal 250
+ * deflaut >> 0xBE >> decimal 190
+ */
+ int Set_Highest_Voltage(int val_highest_voltage);
+
+
+ // Reglage du return time delay EN MICRO SECONDE 2uSec * val_delay_time
+ // minimum >> 0 us
+ // maximum >> 508 us
+ // deflaut >> 125 us
+ int Set_Delay_Time (int val_delay_time );
+
+
+ /** Set Highest Limit Temperature
+ * @param mode
+ * minimum >> Ox00 >> decimal 0
+ * maximum >> 0x96 >> decimal 150
+ */
+ int Set_Temperature_Max (int val_temperature );
+
+
+
+ /** Set the state of LED
+ * @param mode
+ * 0 = off, default
+ * 1 = on
+ */
+ int Set_Etat_LED(int etat);
+
+
+
+ /** Set the mode of the servo
+ * @param mode
+ * 0 = Positional, default
+ * 1 = Continuous rotation
+ */
+ int Set_Mode(int mode);
+
+ /** Set baud rate of all attached servos
+ * @param mode
+ * 0x01 = 1,000,000 bps
+ * 0x03 = 500,000 bps
+ * 0x04 = 400,000 bps
+ * 0x07 = 250,000 bps
+ * 0x09 = 200,000 bps
+ * 0x10 = 115,200 bps
+ * 0x22 = 57,600 bps
+ * 0x67 = 19,200 bps
+ * 0xCF = 9,600 bp
+ */
+ int Set_Baud(int baud);
+
+
+ /** Set goal angle in integer degrees, in positional mode
+ *
+ * @param degrees 0-300
+ * @param flags, defaults to 0
+ * flags[0] = blocking, return when goal position reached
+ * flags[1] = register, activate with a broadcast trigger
+ *
+ */
+ int Set_Goal(int degrees, int flags = 0);
+
+ int Set_Goal_speed(int degrees, int flags = 0);
+
+ /** Set the speed of the servo in continuous rotation mode
+ *
+ * @param speed, -1.0 to 1.0
+ * -1.0 = full speed counter clock wise
+ * 1.0 = full speed clock wise
+ */
+ int Set_CR_Speed(float speed);
+
+
+ /** Permet de définir l'angle limite minimum de l'actionneur ( prend une valeur d'entrée en degres )
+ // minimum >> 0°
+ // maximum >> 300°
+ // deflaut >> 0°
+ */
+ int Set_CW_Angle_Limit(int degrees);
+
+ /** Permet de définir l'angle limite maximum de l'actionneur ( prend une valeur d'entrée en degres )
+ // minimum >> 0°
+ // maximum >> 300°
+ // deflaut >> 300°
+ */
+ int Set_CCW_Angle_Limit(int degrees);
+
+ // Change the ID
+
+ /** Change the ID of a servo
+ *
+ * @param CurentID 1-255
+ * @param NewID 1-255
+ *
+ * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID.
+ * In this situation, only one servo should be connected to the bus
+ */
+ int Set_ID(int CurrentID, int NewID);
+
+
+ /** Poll to see if the servo is moving
+ *
+ * @returns true is the servo is moving
+ */
+ int isMoving(void);
+
+ /** Send the broadcast "trigger" command, to activate any outstanding registered commands
+ */
+ void trigger(void);
+
+ /** Send the "reset" command, to activate any outstanding registered commands
+ */
+ void reset();
+
+ /** Read the current angle of the servo
+ *
+ * @returns float in the range 0.0-300.0
+ */
+ float Get_Position();
+
+ /** Read the temperature of the servo
+ *
+ * @returns float temperature
+ */
+ float Get_Temp(void);
+
+ /** Read the supply voltage of the servo
+ *
+ * @returns float voltage
+ */
+ float Get_Volts(void);
+
+ // Lecture du couple maximum ( retourne la valeur du registre Max Torque de l'AX12 )
+ int Get_Max_Torque (void);
+
+
+ int read(int ID, int start, int length, char* data);
+ int write(int ID, int start, int length, char* data, int flag=0);
+ void multiple_goal_and_speed(int number_ax12,char* tab);
+ float read_and_test(float angle,char* Tab);
+
+private :
+
+ SerialHalfDuplex _ax12;
+ int _ID;
+ int _baud;
+
+
+};
+
+
+/* PROTOTYPES DE FONCTIONS ET POINTEURS */
+
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx */
+/* DESCRIPTION : Fonction de traitement des messages CAN */
+/****************************************************************************************/
+void canProcessRx(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: canRx_ISR */
+/* DESCRIPTION : Interruption en réception sur le CAN */
+/****************************************************************************************/
+void canRx_ISR (void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: SendRawId */
+/* DESCRIPTION : Fonction qui permet d'envoi une trame vide à un ID */
+/****************************************************************************************/
+void SendRawId (unsigned short id);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Fin_action */
+/* DESCRIPTION : Fonction qui confirme la fin de mouvement des AX12 */
+/****************************************************************************************/
+void Fin_action(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Automate_ax12 */
+/* DESCRIPTION : Fonction qui gère les différentes actions des AX12 */
+/****************************************************************************************/
+void AX12_automate(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Initialisation_position */
+/* DESCRIPTION : Fonction qui place les bras en position verticale */
+/****************************************************************************************/
+void Initialisation_position(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_prise */
+/* DESCRIPTION : Fonction qui prepare le robot pour prendre les modules */
+/****************************************************************************************/
+void Preparation_prise(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_haut */
+/* DESCRIPTION : Fonction qui prend et stocke les modules dans la position haute */
+/****************************************************************************************/
+void Stockage_haut(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Stockage_bas */
+/* DESCRIPTION : Fonction qui prend et stocke un module dans la pince */
+/****************************************************************************************/
+void Stockage_bas(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Deposer */
+/* DESCRIPTION : Fonction qui permet de déposer un module */
+/****************************************************************************************/
+void Deposer(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_depos_bas */
+/* DESCRIPTION : Fonction qui prépare le depos d'un module en bas */
+/****************************************************************************************/
+void Preparation_depot_bas(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_depos_haut */
+/* DESCRIPTION : Fonction qui prépare le depos d'un module en haut */
+/****************************************************************************************/
+void Preparation_depot_haut(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Pousser_module */
+/* DESCRIPTION : Fonction qui permet pousser le module situé à l'entrée de la bas */
+/****************************************************************************************/
+void Pousser_module(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Initialisation_gauche */
+/* DESCRIPTION : Fonction qui permet de placer le cote gauche en position initiale */
+/****************************************************************************************/
+void Initialisation_gauche(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparation_prise_gauche */
+/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */
+/****************************************************************************************/
+void Preparation_prise_gauche(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Prendre_module_gauche */
+/* DESCRIPTION : Fonction qui permet prendre un module sur le cote gauche */
+/****************************************************************************************/
+void Prendre_module_gauche(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Tourner_module_gauche */
+/* DESCRIPTION : Fonction qui permet de tourner les modules a gauche */
+/****************************************************************************************/
+void Tourner_module_gauche(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: Preparatio_module_gauche */
+/* DESCRIPTION : Fonction qui prepare le tournante */
+/****************************************************************************************/
+void Preparation_module_gauche(void);
+
+
+#endif
--- a/Globals/constantes.h Fri Mar 31 16:20:26 2017 +0000
+++ b/Globals/constantes.h Thu May 11 12:55:52 2017 +0000
@@ -15,6 +15,8 @@
#define CYLINDRE 61
#define MODULE 62
+#define NOMBRE_OBJETS 16
+
/****
** Variable à modifier en fonction du robot
***/
@@ -23,22 +25,26 @@
#ifdef ROBOT_BIG
- #define NOMBRE_CARTES 1//Le nombre de carte présente sur le gros robot
+ #define NOMBRE_CARTES 2//Le nombre de carte présente sur le gros robot
//#define POSITION_DEBUT_X 1830
//#define POSITION_DEBUT_Y 900
//#define POSITION_DEBUT_T 180
- #define POSITION_DEBUT_X 0
- #define POSITION_DEBUT_Y 0
+ #define POSITION_DEBUT_X 2000-1830
+ #define POSITION_DEBUT_Y 3000-900
#define POSITION_DEBUT_T 0
+ //#define POSITION_DEBUT_X 0
+ //#define POSITION_DEBUT_Y 0
+ //#define POSITION_DEBUT_T 0
+
#define BALISE_TIMEOUT 6000
#else
- #define NOMBRE_CARTES 3 //Le nombre de carte présente sur le petit robot
- #define POSITION_DEBUT_X 990
- #define POSITION_DEBUT_Y 150
+ #define NOMBRE_CARTES 1 //Le nombre de carte présente sur le petit robot
+ #define POSITION_DEBUT_X 0
+ #define POSITION_DEBUT_Y 0
#define POSITION_DEBUT_T 0
#define BALISE_TIMEOUT 2000
--- a/Globals/global.h Fri Mar 31 16:20:26 2017 +0000 +++ b/Globals/global.h Thu May 11 12:55:52 2017 +0000 @@ -2,7 +2,7 @@ #include "constantes.h" #include "ident_crac.h" #include "Display.h" -#include "AX12-V2.h" +#include "AX12.h" #include "Instruction.h" #include "debug.h" #include "Asservissement.h" @@ -25,4 +25,4 @@ extern unsigned short waitingAckID;//L'id du ack attendu extern unsigned short waitingAckFrom;//La provenance du ack attendu -extern char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre +extern char modeTelemetre; // Si à 1, indique que l'on attend une reponse du telemetre \ No newline at end of file
--- a/Globals/ident_crac.h Fri Mar 31 16:20:26 2017 +0000 +++ b/Globals/ident_crac.h Thu May 11 12:55:52 2017 +0000 @@ -27,7 +27,7 @@ #define ODOMETRIE_BIG_VITESSE 0x027 // Odométrie vitesse (Indication sur l'état actuel) #define ODOMETRIE_SMALL_POSITION 0x028 // Odométrie position robot (Position actuel du robot) #define ODOMETRIE_SMALL_VITESSE 0x029 // Odométrie vitesse (Indication sur l'état actuel) -#define ACTION_BIG_DEMARRAGE 0x02A // Action de départ du GR (Lancement de la trajectoire de départ du GR) +#define ACTION_BIG_DEMARRAGE 0x025 // Action de départ du GR (Lancement de la trajectoire de départ du GR) #define ASSERVISSEMENT_INFO_CONSIGNE 0x1F0 // Info Consigne et Commande moteur #define ASSERVISSEMENT_CONFIG_KPP_DROITE 0x1F1 // Config coef KPP_Droit @@ -45,7 +45,7 @@ #define RESET_ACTIONNEURS 0x033 // Reset actionneurs #define RESET_POMPES 0x034 // Reset pompes #define RESET_AX12 0x035 // Reset AX12 - +#define RESET_TELEMETRE 0x036 // Reset telemetre @@ -58,7 +58,7 @@ #define CHECK_ACTIONNEURS 0x063 // Check actionneurs #define CHECK_POMPES 0x064 // Check pompes #define CHECK_AX12 0x065 // Check AX12 - +#define CHECK_OK_TELEMETRE 0x066 // Check telemetre @@ -69,7 +69,7 @@ #define ALIVE_ACTIONNEURS 0x073 // Alive actionneurs #define ALIVE_POMPES 0x074 // Alive pompes #define ALIVE_AX12 0x075 // Alive AX12 - +#define ALIVE_TELEMETRE 0x076 // Alive telemetre @@ -80,13 +80,15 @@ #define ACKNOWLEDGE_ACTIONNEURS 0x103 // Acknowledge actionneurs #define ACKNOWLEDGE_POMPES 0x104 // Acknowledge pompes #define ACKNOWLEDGE_TELEMETRE 0x105 // Acknowledge telemetre +#define ACKNOWLEDGE_AX12 0x106 // Ack ax12 #define ACKNOWLEDGE_STRAT 0x10A // Acknowledge pompes + #define INSTRUCTION_END_BALISE 0x110 // Fin instruction balise (Indique que l'instruction est terminée) #define INSTRUCTION_END_MOTEUR 0x111 // Fin instruction moteur (Indique que l'instruction est terminée) #define INSTRUCTION_END_IHM 0x112 // Fin instruction ecran tactile (Indique que l'instruction est terminée) #define INSTRUCTION_END_ACTIONNEURS 0x113 // Fin instruction actionneurs (Indique que l'instruction est terminée) - +#define INSTRUCTION_END_AX12 0x116 #define ECRAN_CHOICE_STRAT 0x601 // Choix d'une stratégie (n° strat (1-4)) #define ECRAN_CHOICE_COLOR 0x602 // Couleur (0->Blue;1->Yellow) @@ -124,11 +126,11 @@ #define SERVO_AX12_PROCESS 0x091 // AX12 processChange (Lancer le déplacement des AX12) #define SERVO_AX12_DONE 0x092 // AX12 done (Indique q'un AX12 a terminé son déplacement) #define SERVO_XL320 0x093 // XL320 +#define SERVO_AX12_ACTION 0x96 -#define RESET_TELEMETRE 0x300 + #define TELEMETRE_RECHERCHE_COIN 0x301 -#define TELEMETRE_OBJET_DEVANT 0x302 -#define CHECK_OK_TELEMETRE 0x303 +#define TELEMETRE_OBJET 0x302 #define OBJET_SUR_TABLE 0x304 #define POMPE_PWM 0x9A // pwm des pompes (pwm entre 0 et 100)
--- a/Robots/Config_big.h Fri Mar 31 16:20:26 2017 +0000 +++ b/Robots/Config_big.h Thu May 11 12:55:52 2017 +0000 @@ -3,6 +3,9 @@ /** Fichier de configuration des action specifique au gros robot **/ +#define BRAS_GAUCHE 1 +#define BRAS_DROIT 2 + #define AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE 18 #define AX12_ID_PINCE_ARRIERE_HAUTE_DROITE 15
--- a/Robots/Strategie_big.cpp Fri Mar 31 16:20:26 2017 +0000
+++ b/Robots/Strategie_big.cpp Thu May 11 12:55:52 2017 +0000
@@ -1,3 +1,4 @@
+
#include "StrategieManager.h"
#ifdef ROBOT_BIG
#include "Config_big.h"
@@ -9,8 +10,8 @@
/* DESCRIPTION : Permet de faire la funny action en fin de partie */
/****************************************************************************************/
void doFunnyAction(void) {
- AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION);
- AX12_processChange();
+ /* AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_OPEN,AX12_SPEED_FUNNY_ACTION);
+ AX12_processChange();*/
}
@@ -22,88 +23,138 @@
CANMessage msgTx=CANMessage();
switch(id) {
- case 100://Ouvrir les portes avant
- AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_OUVERTE);
- AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_OUVERTE);
- AX12_processChange();
+ case 100://preparation prise
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=1;
+ if (InversStrat){ // si on est inversé, on echange les bras
+ if (speed == BRAS_GAUCHE) speed = BRAS_DROIT;
+ else speed = BRAS_GAUCHE;
+ }
+ msgTx.data[1]=speed;
+ can1.write(msgTx);
break;
- case 101://Fermer les portes avant
- AX12_setGoal(AX12_ID_PORTE_AVANT_GAUCHE, AX12_ANGLE_PORTE_AVANT_GAUCHE_FERMER);
- AX12_setGoal(AX12_ID_PORTE_AVANT_DROITE, AX12_ANGLE_PORTE_AVANT_DROITE_FERMER);
- AX12_processChange();
+ case 101://stockage haut
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=2;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
- case 102://Remonter le peigne
- AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_UP);
- SendRawId(0x123);
- AX12_processChange();
+ case 102://stockage bas
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=3;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
- case 103://Descendre le peigne
- AX12_setGoal(AX12_ID_PEIGNE, AX12_ANGLE_PEIGNE_DOWN);
- AX12_processChange();
+ case 103://deposer
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=4;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
- case 104://Monter le support ventouse haut
- AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP);
- AX12_processChange();
- break;
- case 105://Descendre le support ventouse haut
- AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_DOWN);
- AX12_processChange();
+ case 104://preparation depot bas
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=5;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
- case 106://Remonter le support du cone arriere
- AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_INSIDE);
- AX12_processChange();
+ case 105://preparation depot haut
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=6;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
- case 107://Descendre le support du cone arriere
- AX12_setGoal(AX12_ID_CONE, AX12_ANGLE_CONE_OUTSIDE);
- AX12_processChange();
+ case 106://pousser module
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=7;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
case 110://Ouvrir la pince arrière haute
- AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 205);
- AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 95);
- AX12_processChange();
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
+
+ // action et le cote selectionné
+ msgTx.data[0]=10;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
case 111://Fermer la pince arrière haute
- AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_GAUCHE, 145);
- AX12_setGoal(AX12_ID_PINCE_ARRIERE_HAUTE_DROITE, 155);
- AX12_processChange();
- /*waitingAckID = AX12_ID_PINCE_ARRIERE_HAUTE_DROITE;
- waitingAckFrom = SERVO_AX12_DONE;*/
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=3;
+
+ // action et le cote selectionné
+ msgTx.data[0]=11;
+ msgTx.data[1]=speed;
+ msgTx.data[2]=angle;
+
+ can1.write(msgTx);
break;
case 112://Ouvrir la pince arrière basse
- AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 215);
- AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 85);
- AX12_processChange();
- break;
- case 113://Fermer la pince arrière basse
- AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_GAUCHE, 145);
- AX12_setGoal(AX12_ID_PINCE_ARRIERE_BASSE_DROITE, 155);
- AX12_processChange();
- /* waitingAckID = AX12_ID_PINCE_ARRIERE_BASSE_DROITE;
- waitingAckFrom = SERVO_AX12_DONE;*/
- break;
+ msgTx.id=SERVO_AX12_ACTION;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=2;
- case 114://Ouvrir les portes arrières
- AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 250);
- AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 50);
- AX12_processChange();
+ // action et le cote selectionné
+ msgTx.data[0]=12;
+ msgTx.data[1]=speed;
+
+ can1.write(msgTx);
break;
- case 115://Fermer les portes arrière
- //AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 145);
- //AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 155);
- AX12_setGoal(AX12_ID_PORTE_ARRIERE_GAUCHE, 142);
- AX12_setGoal(AX12_ID_PORTE_ARRIERE_DROITE, 158);
- AX12_processChange();
- //waitingAckID = AX12_ID_PORTE_ARRIERE_DROITE;
- //waitingAckFrom = SERVO_AX12_DONE;
- break;
-
+
case 120://Activer les pompes
- AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE);
- AX12_processChange();
+ /*AX12_setGoal(AX12_ID_VENTOUSE, AX12_ANGLE_VENTOUSE_UP,AX12_SPEED_VENTOUSE);
+ AX12_processChange();*/
msgTx.id=POMPE_PWM;
@@ -161,14 +212,42 @@
case 40: // demande au telemetre la position d'un objet
//SendRawId(TELEMETRE_RECHERCHE_OBJET);
- waitingAckID = ACKNOWLEDGE_TELEMETRE;
- waitingAckFrom = 0;
- modeTelemetre =1;
+
+ modeTelemetre = 1;
+
+ //angle = angle /10;
+
+ msgTx.id=TELEMETRE_OBJET;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=1;
+ // indice du module sur le terrain
+ msgTx.data[0] = (unsigned char)speed;
+
+ /*
+ // x sur 2 octets
+ msgTx.data[0]=(unsigned char)speed;
+ msgTx.data[1]=(unsigned char)(speed>>8);
+ // y sur 2 octets
+ msgTx.data[2]=(unsigned char)angle;
+ msgTx.data[3]=(unsigned char)(angle>>8);
+ // theta signé sur 2 octets
+ //msgTx.data[4]=(unsigned char)theta;
+ //msgTx.data[5]=(unsigned char)(theta>>8);
+ msgTx.data[4]=0;
+ msgTx.data[5]=0;*/
+
+ can1.write(msgTx);
+
+ break;
case 130://Lancer mouvement de sortie de la zone de départ
- SendRawId(ACTION_BIG_DEMARRAGE);
- waitingAckID = ACTION_BIG_DEMARRAGE;
- waitingAckFrom = INSTRUCTION_END_MOTEUR;
+ msgTx.id=ACTION_BIG_DEMARRAGE;
+ msgTx.format=CANStandard;
+ msgTx.type=CANData;
+ msgTx.len=1;
+ msgTx.data[0] = (unsigned char)speed;
+ can1.write(msgTx);
break;
default:
@@ -186,13 +265,13 @@
void initRobot(void)
{
//Enregistrement de tous les AX12 présent sur la carte
- AX12_register(5, AX12_SERIAL2);
+ /*AX12_register(5, AX12_SERIAL2);
AX12_register(18, AX12_SERIAL2);
AX12_register(13, AX12_SERIAL2);
AX12_register(1, AX12_SERIAL1);
AX12_register(11, AX12_SERIAL1);
AX12_register(8, AX12_SERIAL1);
- AX12_register(7, AX12_SERIAL2);
+ AX12_register(7, AX12_SERIAL2);*/
//AX12_setGoal(AX12_ID_FUNNY_ACTION, AX12_ANGLE_FUNNY_ACTION_CLOSE,AX12_SPEED_FUNNY_ACTION);
//AX12_processChange();
@@ -252,6 +331,7 @@
{
switch(id)
{
+ // strat de match
case 1:
strcpy(cheminFileStart,"/local/strat1.txt");
return FileExists(cheminFileStart);
@@ -283,14 +363,24 @@
strcpy(cheminFileStart,"/local/strat10.txt");
return FileExists(cheminFileStart);
- //
+ // strat de demo
case 0x10:
strcpy(cheminFileStart,"/local/moteur.txt");
return FileExists(cheminFileStart);
case 0x11:
- strcpy(cheminFileStart,"/local/moteur2.txt");
+#ifdef ROBOT_BIG
+ strcpy(cheminFileStart,"/local/bras.txt");
+#else
+ strcpy(cheminFileStart,"/local/porteAvant.txt");
+#endif
return FileExists(cheminFileStart);
-
+ case 0x12:
+#ifdef ROBOT_BIG
+ strcpy(cheminFileStart,"/local/balancier.txt");
+#else
+ strcpy(cheminFileStart,"/local/mainTourneuse.txt");
+#endif
+ return FileExists(cheminFileStart);
default:
strcpy(cheminFileStart,"/local/strat1.txt");
return 0;
--- a/Robots/Strategie_small.cpp Fri Mar 31 16:20:26 2017 +0000
+++ b/Robots/Strategie_small.cpp Thu May 11 12:55:52 2017 +0000
@@ -36,243 +36,78 @@
/****************************************************************************************/
unsigned char doAction(unsigned char id, unsigned short speed, short angle) {
switch(id) {
- case 101://Descendre le bras pour pecher les poissons
+ case 100: // preparation prise bras central
+ Preparation_prise();
+ break;
+ case 101:// stockage haut bras central
+ Stockage_haut();
+ break;
+ case 102:// stockage bas bras central
+ Stockage_bas();
+ break;
+ case 103:// ouvrir la main du bras cental
+ Deposer();
+ break;
+ case 104:// preparation de depot du module bas du bras central
+ Preparation_depot_bas();
+ break;
+ case 105:// preparation de depot du module haut du bras central
+ Preparation_depot_haut();
+ break;
+ case 106:// positionner le bras afin de pousser un module debout
+ Pousser_module();
+ break;
+ case 110:// ouvrir une porte avant
if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_BRAS_BASE_DROIT,
- AX12_ANGLE_BRAS_BASE_DROIT_OUVERT,
- AX12_SPEED_BRAS_BASE
- );
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_DROIT,
- AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
- } else {
- AX12_setGoal(
- AX12_ID_BRAS_BASE_GAUCHE,
- AX12_ANGLE_BRAS_BASE_GAUCHE_OUVERT,
- AX12_SPEED_BRAS_BASE
- );
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_GAUCHE,
- AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
- }
- break;
- case 102://Remonter le bras avec les poissons dessus
- if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_BRAS_BASE_DROIT,
- AX12_ANGLE_BRAS_BASE_DROIT_MOITER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
+ if(speed == 1) speed = 0;
+ else speed = 1;
} else {
- AX12_setGoal(
- AX12_ID_BRAS_BASE_GAUCHE,
- AX12_ANGLE_BRAS_BASE_GAUCHE_MOITER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
- }
- break;
- case 103://Decendre à 30°
- if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_BRAS_BASE_DROIT,
- AX12_ANGLE_BRAS_BASE_DROIT_RELACHER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
- } else {
- AX12_setGoal(
- AX12_ID_BRAS_BASE_GAUCHE,
- AX12_ANGLE_BRAS_BASE_GAUCHE_RELACHER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
- }
- break;
- case 104://ouvrir séparateur
- if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_DROIT,
- AX12_ANGLE_BRAS_RELACHEUR_DROIT_OUVERT,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_RELACHEUR_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
- } else {
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_GAUCHE,
- AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_OUVERT,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_RELACHEUR_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
+
}
break;
- case 105://Rentrer le bras dans le robot, fermer le séparateur
+ case 111:// securiser un module avec une porte avant
if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_DROIT,
- AX12_ANGLE_BRAS_RELACHEUR_DROIT_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
- wait_ms(100);
- AX12_setGoal(
- AX12_ID_BRAS_BASE_DROIT,
- AX12_ANGLE_BRAS_BASE_DROIT_REPLIER,
- AX12_SPEED_BRAS_BASE
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
- } else {
- AX12_setGoal(
- AX12_ID_BRAS_RELACHEUR_GAUCHE,
- AX12_ANGLE_BRAS_RELACHEUR_GAUCHE_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
- wait_ms(100);
- AX12_setGoal(
- AX12_ID_BRAS_BASE_GAUCHE,
- AX12_ANGLE_BRAS_BASE_GAUCHE_REPLIER,
- AX12_SPEED_BRAS_RELACHEUR
- );
- AX12_processChange();
- waitingAckID = AX12_ID_BRAS_BASE_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
+ if(speed == 1) speed = 0;
+ else speed = 1;
+ } else {
+
}
break;
- case 106://descendre l'attrape coquillages au niveau des rochets gauche si non inversé, droit si inversé
+ case 112:// ranger le porte avant dans le robot
if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_PALET_DROIT,
- AX12_ANGLE_PALET_DROIT_ROCHET,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
+ if(speed == 1) speed = 0;
+ else speed = 1;
} else {
- AX12_setGoal(
- AX12_ID_PALET_GAUCHE,
- AX12_ANGLE_PALET_GAUCHE_ROCHET,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
+
}
break;
- case 107://descendre l'attrape coquillages au niveau de la table gauche si non inversé, droit si inversé
+ case 120:// poser une main tourneuse
if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_PALET_DROIT,
- AX12_ANGLE_PALET_DROIT_OUVERT,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
+ if(speed == 1) speed = 0;
+ else speed = 1;
} else {
- AX12_setGoal(
- AX12_ID_PALET_GAUCHE,
- AX12_ANGLE_PALET_GAUCHE_OUVERT,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
- }
- break;
- case 108://Remonter l'attrape coquillages gauche si non inversé, droit si inversé
- if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
- AX12_setGoal(
- AX12_ID_PALET_DROIT,
- AX12_ANGLE_PALET_DROIT_FERMER,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_DROIT;
- waitingAckFrom = SERVO_AX12_DONE;
- } else {
- AX12_setGoal(
- AX12_ID_PALET_GAUCHE,
- AX12_ANGLE_PALET_GAUCHE_FERMER,
- AX12_SPEED_PALET
- );
- AX12_processChange();
- waitingAckID = AX12_ID_PALET_GAUCHE;
- waitingAckFrom = SERVO_AX12_DONE;
+
}
break;
-
- case 200://ouvir la pince gauche
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
+ case 121:// relever une main tourneuse
+ if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+ if(speed == 1) speed = 0;
+ else speed = 1;
} else {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
- }
- break;
- case 205://Sécuriser le palet gauche
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
- } else {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
- }
- break;
- case 202://fermer pince gauche
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
- } else {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+
}
break;
- case 201://ouvir la pince droite
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_OUVERTE);
+ case 122:// tourner un module
+ if(InversStrat == 1) {//Si c'est inversé, On utilise le bras coté droit
+ if(speed == 1) speed = 0;
+ else speed = 1;
} else {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_OUVERTE);
+
}
break;
- case 204://Sécuriser le palet droit
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
- } else {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
- }
- break;
- case 203://fermer pince droite
- if(InversStrat == 1) {
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
- } else {
- XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
- }
- break;
+
case 10://Désactiver le stop
isStopEnable = 0;
break;
@@ -302,12 +137,12 @@
/**
On enregistre les id des AX12 présent sur la carte
**/
- AX12_register(1,AX12_SERIAL1,0x0FF);
+ /*AX12_register(1,AX12_SERIAL1,0x0FF);
AX12_register(2,AX12_SERIAL1);
AX12_register(18,AX12_SERIAL1);
AX12_register(4,AX12_SERIAL2);
AX12_register(16,AX12_SERIAL2);
- AX12_register(17,AX12_SERIAL2,0x0FF);
+ AX12_register(17,AX12_SERIAL2,0x0FF);*/
//runRobotTest();
@@ -325,7 +160,7 @@
/****************************************************************************************/
void initRobotActionneur(void)
{
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
+ /*XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_FERMER);
XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_FERMER);
AX12_setGoal(
@@ -358,7 +193,7 @@
AX12_ANGLE_PALET_GAUCHE_FERMER,
AX12_SPEED_PALET
);
- AX12_processChange();
+ AX12_processChange(); */
}
/****************************************************************************************/
@@ -367,7 +202,7 @@
/****************************************************************************************/
void runRobotTest(void)
{
- XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
+ /*XL320_setGoal(XL320_ID_PINCE_GAUCHE, XL320_ANGLE_PINCE_GAUCHE_SECURISE);
XL320_setGoal(XL320_ID_PINCE_DROITE, XL320_ANGLE_PINCE_DROITE_SECURISE);
wait_ms(200);
@@ -456,7 +291,7 @@
AX12_ANGLE_PALET_GAUCHE_FERMER,
AX12_SPEED_PALET
);
- AX12_processChange();
+ AX12_processChange();*/
}
/****************************************************************************************/
--- a/Strategie/Strategie.cpp Fri Mar 31 16:20:26 2017 +0000
+++ b/Strategie/Strategie.cpp Thu May 11 12:55:52 2017 +0000
@@ -25,13 +25,17 @@
//unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS,CHECK_AX12,CHECK_POMPES};
//unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS,ALIVE_AX12,ALIVE_POMPES};
+unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR,CHECK_MOTEUR};
+unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR,ALIVE_MOTEUR};
+
+InterruptIn jack(p25); // entrée analogique en interruption pour le jack
+#else
+//unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
+//unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
+
unsigned short id_check[NOMBRE_CARTES]= {CHECK_MOTEUR};
unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_MOTEUR};
-InterruptIn jack(p25); // entrée analogique en interruption pour le jack
-#else
-unsigned short id_check[NOMBRE_CARTES]= {CHECK_BALISE,CHECK_MOTEUR,CHECK_ACTIONNEURS};
-unsigned short id_alive[NOMBRE_CARTES]= {ALIVE_BALISE,ALIVE_MOTEUR,ALIVE_ACTIONNEURS};
InterruptIn jack(p24); // entrée analogique en interruption pour le jack
#endif
unsigned char checkCurrent = 0;
@@ -315,6 +319,13 @@
localData5 = 0;
}
}
+
+ if(InversStrat == 1) {
+ /*if (instruction.direction == FORWARD) instruction.direction = BACKWARD;
+ else instruction.direction = FORWARD;*/
+ instruction.direction = ((instruction.direction == FORWARD)?BACKWARD:FORWARD);
+ }
+
localData2 = (((instruction.direction == FORWARD)?1:-1)*instruction.arg1);
GoStraight(localData2, 0, 0, localData5);
@@ -379,23 +390,23 @@
GoStraight(localData2, localData5, localData3, 0);
break;
case ACTION:
- SendRawId(0x300);
- waitingAckFrom = 0;
- waitingAckID = 0;
+
+ waitingAckID = SERVO_AX12_ACTION;
+ waitingAckFrom = ACKNOWLEDGE_AX12;
if(doAction(instruction.arg1,instruction.arg2,instruction.arg3)) {
//L'action est spécifique
if((waitingAckFrom == 0 && waitingAckID == 0) || instruction.nextActionType == ENCHAINEMENT) {
- ///wait_us(200);
+ wait_us(200);
actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
} else {
- gameEtat = ETAT_GAME_WAIT_END_INSTRUCTION;
+ gameEtat = ETAT_GAME_WAIT_ACK;
}
return;
} else {
//C'est un AX12 qu'il faut bouger
- AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
- AX12_enchainement++;
+ //AX12_setGoal(instruction.arg1,instruction.arg3/10,instruction.arg2);
+ //AX12_enchainement++;
}
break;
@@ -412,12 +423,17 @@
cartesCheker.reset();//On reset le timeOut
cartesCheker.start();
if(AX12_enchainement > 0) {
- AX12_processChange();//Il faut lancer le déplacement des AX12
- AX12_enchainement = 0;
+ //AX12_processChange();//Il faut lancer le déplacement des AX12
+ //AX12_enchainement = 0;
}
} else {//C'est un enchainement
- actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
- gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
+ if(instruction.order == MV_LINE){
+ gameEtat = ETAT_GAME_WAIT_ACK;
+
+ }else{
+ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//C'est un enchainement, on charge directement l'instruction suivante
+ }
}
break;
@@ -461,16 +477,15 @@
waitingAckFrom = INSTRUCTION_END_MOTEUR;
break;
case ACTION:
- SendRawId(0x302);
- waitingAckFrom = 0; //SERVO_AX12_DONE;
- waitingAckID = 0;// instruction.arg1;
- /*if (modeTelemetre == 0){
-
- }else{ // si on attend la reponse du telemetre
- waitingAckFrom = 0;//TELEMETRE_RECHERCHE_OBJET;
- waitingAckID = OBJET_SUR_TABLE;
- //modeTelemetre = 0;
- }*/
+
+ if (modeTelemetre == 0){
+ waitingAckID = SERVO_AX12_ACTION;// instruction.arg1;
+ waitingAckFrom = INSTRUCTION_END_AX12; //SERVO_AX12_DONE;
+ }else{ // si on attend la reponse du telemetre
+ //modeTelemetre = 1;
+ waitingAckID = OBJET_SUR_TABLE;
+ waitingAckFrom = 0;
+ }
break;
default:
break;
@@ -497,8 +512,47 @@
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
}
break;
+
+ case ETAT_GAME_JUMP_CONFIG:
+ signed int depasX = 1, depasY = 1; // servent à indiquer le sens de dépassement des coordonnées
+ // 1 si l'instruction est plus grande que la position du robot
+ // -1 si l'instruction est plus petite que la position du robot
+ // 0 si l'instruction et position du robot sont proche de moins de 1cm
+ if (abs(x_robot-instruction.JumpTimeOrX)<10){
+ depasX = 0;
+ }else if(x_robot > instruction.JumpTimeOrX){
+ depasX = -1;
+ }
+
+ if(abs(y_robot-instruction.JumpY)<10){
+ depasY = 0;
+ }else if(y_robot > instruction.JumpY){
+ depasY = -1;
+ }
+
+ gameEtat = ETAT_GAME_JUMP_POSITION;
+ break;
case ETAT_GAME_JUMP_POSITION:
-
+ bool Xok = false, Yok = false;
+
+ if (depasX == 0){
+ Xok = true;
+ }else if ((instruction.JumpTimeOrX - x_robot)*depasX < -5){
+ Xok = true;
+ }
+
+ if (depasY == 0){
+ Yok = true;
+ }else if ((instruction.JumpY - y_robot)*depasY < -5){
+ Yok = true;
+ }
+
+ // on teste si les deux coordonnées ont été dépassées, si oui on lance l'instruction suivante
+ if (Xok && Yok){
+ actual_instruction = instruction.nextLineOK;//On indique que l'on va charger l'instruction suivante
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;//On charge l'instruction suivante
+ }
+
break;
case ETAT_GAME_WAIT_END_INSTRUCTION:
if(waitingAckID == 0 && waitingAckFrom ==0) {//On attend que la carte nous indique que l'instruction est terminée
@@ -522,7 +576,7 @@
break;
case ETAT_WARNING_END_LAST_INSTRUCTION://trouver le meilleur moyen de reprendre l'instruction en cours
#ifdef ROBOT_BIG
- actual_instruction = 2;//Modification directe... c'est pas bien mais ça marchait pour le match 5
+ actual_instruction = instruction.nextLineError;// 2 //Modification directe... c'est pas bien mais ça marchait pour le match 5
gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
#else
actual_instruction = instruction.nextLineError;
@@ -586,18 +640,21 @@
if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id) {
waitingAckFrom = 0;//C'est la bonne carte qui indique qu'elle est en ligne
}
- break;
+ break;
case ACKNOWLEDGE_BALISE:
case ACKNOWLEDGE_MOTEUR:
case ACKNOWLEDGE_IHM:
case ACKNOWLEDGE_TELEMETRE:
+ case ACKNOWLEDGE_AX12:
case INSTRUCTION_END_BALISE:
case INSTRUCTION_END_MOTEUR:
case INSTRUCTION_END_IHM:
+ case INSTRUCTION_END_AX12:
if(waitingAckFrom == msgRxBuffer[FIFO_lecture].id && ((unsigned short)msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8) == waitingAckID)) {
waitingAckFrom = 0;
waitingAckID = 0;
+ SendRawId(0x123);
}
break;
#ifdef ROBOT_BIG
@@ -628,11 +685,11 @@
break;
case SERVO_AX12_DONE:
- SendRawId(0x666);
+ SendRawId(POMPE_PWM);
/*//SendAck(0x114, SERVO_AX12_DONE);
AX12_notifyCANEnd(((unsigned short)(msgRxBuffer[FIFO_lecture].data[0])));
- /*gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
+ gameEtat = ETAT_GAME_LOAD_NEXT_INSTRUCTION;
waitingAckFrom = 0;
waitingAckID = 0;*/
@@ -654,10 +711,6 @@
}
break;
-
- case TELEMETRE_RECHERCHE_COIN:
- SendRawId(RESET_TELEMETRE);
- break;
case ECRAN_CHOICE_STRAT://Choix du fichier de stratégie à utiliser
if(gameEtat == ETAT_CONFIG) {//C'est bon on a le droit de modifier les config
@@ -724,10 +777,19 @@
break;
case OBJET_SUR_TABLE:
- if ((gameEtat == ETAT_GAME_WAIT_ACK)&&(modeTelemetre)){
- strat_instructions[actual_instruction+1].arg1 = msgRxBuffer[FIFO_lecture].data[0]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[1])<<8);
- strat_instructions[actual_instruction+1].arg2 = msgRxBuffer[FIFO_lecture].data[2]|((unsigned short)(msgRxBuffer[FIFO_lecture].data[3])<<8);
- }
+ if (msgRxBuffer[FIFO_lecture].data[1] == 0xff){
+
+ gameEtat = ETAT_WARNING_END_LAST_INSTRUCTION;
+ }
+ else{
+
+ waitingAckFrom = 0;
+ waitingAckID = 0;
+
+ strat_instructions[actual_instruction+1].arg1 = returnX(strat_instructions[actual_instruction].arg2);
+ strat_instructions[actual_instruction+1].arg2 = returnY(strat_instructions[actual_instruction].arg2);
+ }
+ modeTelemetre = 0;
break;
}
--- a/Strategie/Strategie.h Fri Mar 31 16:20:26 2017 +0000
+++ b/Strategie/Strategie.h Thu May 11 12:55:52 2017 +0000
@@ -20,6 +20,7 @@
ETAT_GAME_PROCESS_INSTRUCTION,
ETAT_GAME_WAIT_ACK,
ETAT_GAME_JUMP_TIME,
+ ETAT_GAME_JUMP_CONFIG,
ETAT_GAME_JUMP_POSITION,
ETAT_GAME_WAIT_END_INSTRUCTION,
--- a/Telemetre/Telemetre.cpp Fri Mar 31 16:20:26 2017 +0000
+++ b/Telemetre/Telemetre.cpp Thu May 11 12:55:52 2017 +0000
@@ -1,9 +1,7 @@
# include "Telemetre.h"
-#define TELEMETRE_PROFONDEUR 50
-T_LISTE_MODULES listeModules;
-
+T_MODULE listeModules[NOMBRE_OBJETS];
/*********************************************************************************************************/
/* FUNCTION NAME: SendTelemetreID */
/* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
@@ -44,48 +42,63 @@
}
void initModules(void){
-
-
- /*listeModules ={true, 1350, 0},
- {true, 0, 1150}};*/
-
- listeModules.module1.x=600;
- listeModules.module1.y=1000;
- listeModules.module1.existe = true;
- listeModules.module1.timeout = 0;
-
- listeModules.module2.x=600;
- listeModules.module2.y=200;
- listeModules.module2.existe = true;
- listeModules.module2.timeout = 0;
-
- listeModules.module3.x=1100;
- listeModules.module3.y=500;
- listeModules.module3.existe = true;
- listeModules.module3.timeout = 0;
+
+ listeModules[0].x = 0;
+ listeModules[0].y = 1100;
- listeModules.module4.x=1400;
- listeModules.module4.y=900;
- listeModules.module4.existe = true;
- listeModules.module4.timeout = 0;
-
- listeModules.module5.x=1850;
- listeModules.module5.y=800;
- listeModules.module5.existe = true;
- listeModules.module5.timeout = 0;
+ listeModules[1].x = 200;
+ listeModules[1].y = 950;
+
+ listeModules[2].x = 600;
+ listeModules[2].y = 200;
+
+ listeModules[3].x = 600;
+ listeModules[3].y = 1000;
+
+ listeModules[4].x = 1100;
+ listeModules[4].y = 500;
+
+ listeModules[5].x = 1350;
+ listeModules[5].y = 0;
+
+ listeModules[6].x = 1400;
+ listeModules[6].y = 900;
+
+ listeModules[7].x = 1850;
+ listeModules[7].y = 800;
+
+ listeModules[8].x = 0;
+ listeModules[8].y = 1850;
+
+ listeModules[9].x = 200;
+ listeModules[9].y = 2050;
+
+ listeModules[10].x = 600;
+ listeModules[10].y = 2000;
+
+ listeModules[11].x = 600;
+ listeModules[11].y = 2800;
- listeModules.fusee1.x = 1350;
- listeModules.fusee1.y = 0;
- listeModules.fusee1.nb_module = 4;
- listeModules.fusee1.timeout = 0 ;
+ listeModules[12].x = 1100;
+ listeModules[12].y = 2500;
+
+ listeModules[13].x = 1350;
+ listeModules[13].y = 0;
- listeModules.fusee2.x = 0;
- listeModules.fusee2.y = 1150;
- listeModules.fusee2.nb_module = 4;
- listeModules.fusee2.timeout = 0;
-
+ listeModules[14].x = 1400;
+ listeModules[14].y = 2100;
+
+ listeModules[15].x = 1850;
+ listeModules[15].y = 2200;
}
+signed short returnX(int indiceTab){
+ return listeModules[indiceTab].x;
+ }
+
+signed short returnY(int indiceTab){
+ return listeModules[indiceTab].y;
+ }
void processData(int objectType, signed short x, signed short y, int nb_module){
switch(objectType){
@@ -95,10 +108,10 @@
case MODULE:
//
- if( (x == listeModules.module1.x) && (y == listeModules.module1.y) ){
+ /*if( (x == listeModules.module1.x) && (y == listeModules.module1.y) ){
printf("module 1");
listeModules.module1.timeout ++;
- }
+ } */
break;
case BALLE:
--- a/Telemetre/Telemetre.h Fri Mar 31 16:20:26 2017 +0000
+++ b/Telemetre/Telemetre.h Thu May 11 12:55:52 2017 +0000
@@ -3,33 +3,15 @@
#include "global.h"
+#define TELEMETRE_PROFONDEUR 50
- struct T_MODULE{
- bool existe;
- signed short x;
- signed short y;
- int timeout;
- };
-
-struct T_FUSEE{
- int nb_module;
+
+
+struct T_MODULE{
signed short x;
signed short y;
- int timeout;
};
-struct T_LISTE_MODULES{
- // cote bleu
- T_MODULE module1; /// 1000 y , 600 x
- T_MODULE module2; /// 200 y , 600 x
- T_MODULE module3; /// 500 y , 1100 x
- T_MODULE module4; /// 900 y , 1400 x
- T_MODULE module5; /// 800 y , 1850 x
- T_FUSEE fusee1 ; /// 0 y , 1350 x
- T_FUSEE fusee2 ; /// 1150 y , 0 x
- };
-
-
/*********************************************************************************************************/
/* FUNCTION NAME: SendTelemetreID */
/* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
@@ -43,4 +25,7 @@
void TraitementCylindreMultiple(void);
+signed short returnX(int indiceTab);
+
+signed short returnY(int indiceTab);
#endif
\ No newline at end of file
--- a/main.cpp Fri Mar 31 16:20:26 2017 +0000
+++ b/main.cpp Thu May 11 12:55:52 2017 +0000
@@ -52,6 +52,6 @@
while(true) {
automate_process();//Boucle dans l'automate principal
canProcessRx();//Traitement des trames CAN en attente
- AX12_doLoop();//Vérification de la position des AX12
+ //AX12_doLoop();//Vérification de la position des AX12
}
}