CRAC Team
/
CRAC-Strat_2017
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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
}
}