CRAC Team
code de la carte à tout faire / carte strat
Dependencies: mbed
Revision 0:76bc3ed27822, committed 2017-05-10
- Comitter:
- ClementBreteau
- Date:
- Wed May 10 09:10:26 2017 +0000
- Commit message:
- pour gatien le 10 5 2017
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.cpp Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,1577 @@
+/* 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 MAX_TIMEOUT 500
+
+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 )
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.h Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,429 @@
+/* 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 "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;
+
+
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12_V2/AX12_V2.cpp Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,24 @@
+#include "AX12_V2.h"
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: AX12BaisserMainGauche */
+/* DESCRIPTION : envoie valeurActuelle+1 à l'AX12 des mains GAUCHE */
+/****************************************************************************************/
+char AX12BaisserMainGauche(char valeurActuelle){
+ //envoie valeurActuelle+1 à l'AX12 des mains GAUCHE
+ valeurActuelle += 10;
+ myAX12_2->Set_Secure_Goal(valeurActuelle);
+ return valeurActuelle;
+ }
+
+/****************************************************************************************/
+/* FUNCTION NAME: AX12BaisserMainDroite */
+/* DESCRIPTION : envoie valeurActuelle+1 à l'AX12 des mains DROITE */
+/****************************************************************************************/
+char AX12BaisserMainDroite(char valeurActuelle){
+ //envoie valeurActuelle+1 à l'AX12 des mains DROITE
+ valeurActuelle += 10;
+ myAX12_2->Set_Secure_Goal(valeurActuelle);
+ return valeurActuelle;
+ }
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AX12_V2/AX12_V2.h Wed May 10 09:10:26 2017 +0000 @@ -0,0 +1,18 @@ +#ifndef CRAC_AX12_V2 +#define CRAC_AX12_V2 + +#include "global.h" + +/****************************************************************************************/ +/* FUNCTION NAME: AX12BaisserMainGauche */ +/* DESCRIPTION : envoie valeurActuelle+1 à l'AX12 des mains GAUCHE */ +/****************************************************************************************/ +char AX12BaisserMainGauche(char valeurActuelle); +/****************************************************************************************/ +/* FUNCTION NAME: AX12BaisserMainDroite */ +/* DESCRIPTION : envoie valeurActuelle+1 à l'AX12 des mains DROITE */ +/****************************************************************************************/ +char AX12BaisserMainDroite(char valeurActuelle); + + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/Actionneurs.cpp Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,28 @@
+#include "Actionneurs.h"
+
+DigitalOut electroVanne(PA_6);
+
+/****************************************************************************************/
+/* FUNCTION NAME: turbineWritePWM */
+/* DESCRIPTION : envoie une pwm de valeurPWM à la turbine */
+/****************************************************************************************/
+void turbineWritePWM(int valeurPWM){
+ // envoie une pwm de valeurPWM à la turbine
+ }
+
+/****************************************************************************************/
+/* FUNCTION NAME: pompeWritePWM */
+/* DESCRIPTION : envoie une pwm de valeurPWM à la pompe */
+/****************************************************************************************/
+void pompeWritePWM(int valeurPWM){
+ // envoie une pwm de valeurPWM à la pompe
+ }
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: controlElectrovanne */
+/* DESCRIPTION : permet de controller l'electrovanne */
+/****************************************************************************************/
+void controlElectrovanne(int etat){
+ electroVanne = 1;
+ }
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Actionneurs/Actionneurs.h Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,47 @@
+#ifndef CRAC_ACTIONNEUR
+#define CRAC_ACTIONNEUR
+
+#include "global.h"
+
+typedef enum
+{
+ ETAT_ATTENTE,
+
+ ETAT_TRAITEMENT_ACTION_AX12,
+
+ ETAT_BAISSER_MAIN_GAUCHE,
+ ETAT_BAISSER_MAIN_DROITE,
+ ETAT_CHECK_COULEUR,
+
+ ETAT_TURBINE,
+ ETAT_POMPES
+
+} E_GameEtat;
+
+
+typedef enum{
+ BLEU,
+ JAUNE
+}E_Couleur;
+
+/****************************************************************************************/
+/* FUNCTION NAME: turbineWritePWM */
+/* DESCRIPTION : envoie une pwm de valeurPWM à la turbine */
+/****************************************************************************************/
+void turbineWritePWM(int valeurPWM);
+/****************************************************************************************/
+/* FUNCTION NAME: pompeWritePWM */
+/* DESCRIPTION : envoie une pwm de valeurPWM à la pompe */
+/****************************************************************************************/
+void pompeWritePWM(int valeurPWM);
+/****************************************************************************************/
+/* FUNCTION NAME: pompeWritePWM */
+/* DESCRIPTION : ouvre ou ferme l'electrovannes */
+/****************************************************************************************/
+void electrovanneControle(int etat);
+/****************************************************************************************/
+/* FUNCTION NAME: pompeWritePWM */
+/* DESCRIPTION : ouvre ou ferme l'electrovannes */
+/****************************************************************************************/
+void electrovanneControle(int etat);
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Globals/constantes.h Wed May 10 09:10:26 2017 +0000 @@ -0,0 +1,8 @@ +#ifndef CRAC_CONSTANTES +#define CRAC_CONSTANTES + +#define SIZE_FIFO 20 +#define ENFONCER 0 +#define AX12_POSIOTION_BASSE 0x100 + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Globals/global.h Wed May 10 09:10:26 2017 +0000 @@ -0,0 +1,14 @@ +#include "mbed.h" + +#include "SerialHalfDuplex.h" + +#include "constantes.h" +#include "ident_crac.h" +#include "Actionneurs.h" +#include "AX12.h" +#include "AX12_V2.h" + + +extern CAN can1; +extern CANMessage msgRxBuffer[SIZE_FIFO]; +extern unsigned char FIFO_ecriture; \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Globals/ident_crac.h Wed May 10 09:10:26 2017 +0000 @@ -0,0 +1,138 @@ +#ifndef CRAC_IDENTH +#define CRAC_IDENTH + + + +#define GLOBAL_GAME_END 0x004 // Stop fin du match +#define GLOBAL_START 0x002 // Start +#define GLOBAL_END_INIT_POSITION 0x005 // Fin positionnement robot avant depart +#define GLOBAL_FUNNY_ACTION 0x007 // Funny action start (0: start, 1: stop) + +#define BALISE_STOP 0x003 // Trame stop + +#define BALISE_DANGER 0xA // Trame danger + +#define BALISE_END_DANGER 0xB // Trame fin de danger + + +#define ASSERVISSEMENT_STOP 0x001 // Stop moteur +#define ASSERVISSEMENT_SPEED_DANGER 0x006 // Vitesse de danger +#define ASSERVISSEMENT_XYT 0x020 // Asservissement (x,y,theta) (0 : au choix 1 : avant -1 : arrière) +#define ASSERVISSEMENT_COURBURE 0x021 // Asservissement rayon de courbure (+ gauche, - droite , sens : 1avt , -1arr; enchainement => 1 oui, 0 => non, 2=>derniére instruction de l'enchainement) +#define ASSERVISSEMENT_CONFIG 0x022 // Asservissement paramètre (définir les valeurs de vitesse max et d'eccélération max) +#define ASSERVISSEMENT_ROTATION 0x023 // Asservissement rotation +#define ASSERVISSEMENT_RECALAGE 0x024 // Moteur tout droit (recalage : 0 mouvement seul, 1 x, 2y valeur : coordonnée à laquelle est recalé x/y; enchainement => 1 oui, 0 => non) + +#define ODOMETRIE_BIG_POSITION 0x026 // Odométrie position robot (Position actuel du robot) +#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 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 +#define ASSERVISSEMENT_CONFIG_KPI_DROITE 0x1F2 // Config coef KPI_Droit +#define ASSERVISSEMENT_CONFIG_KPD_DROITE 0x1F3 // Config coef KPD_Droit +#define ASSERVISSEMENT_CONFIG_KPP_GAUCHE 0x1F4 // Config coef KPP_Gauche +#define ASSERVISSEMENT_CONFIG_KPI_GAUCHE 0x1F5 // Config coef KPI_Gauche +#define ASSERVISSEMENT_CONFIG_KPD_GAUCHE 0x1F6 // Config coef KPD_Gauche +#define ASSERVISSEMENT_ENABLE 0x1F7 // Activation asservissement (0 : désactivation, 1 : activation) + + +#define RESET_BALISE 0x030 // Reset balise +#define RESET_MOTEUR 0x031 // Reset moteur +#define RESET_IHM 0x032 // Reset écran tactile +#define RESET_ACTIONNEURS 0x033 // Reset actionneurs +#define RESET_POMPES 0x034 // Reset pompes +#define RESET_AX12 0x035 // Reset AX12 +#define RESET_TELEMETRE 0x036 // Reset telemetre + + + +#define RESET_STRAT 0x3A // Reset stratégie + + +#define CHECK_BALISE 0x060 // Check balise +#define CHECK_MOTEUR 0x061 // Check moteur +#define CHECK_IHM 0x062 // Check écran tactile +#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 + + + + +#define ALIVE_BALISE 0x070 // Alive balise +#define ALIVE_MOTEUR 0x071 // Alive moteur +#define ALIVE_IHM 0x072 // Alive écran tactile +#define ALIVE_ACTIONNEURS 0x073 // Alive actionneurs +#define ALIVE_POMPES 0x074 // Alive pompes +#define ALIVE_AX12 0x075 // Alive AX12 +#define ALIVE_TELEMETRE 0x076 // Alive telemetre + + + + +#define ACKNOWLEDGE_BALISE 0x100 // Acknowledge balise +#define ACKNOWLEDGE_MOTEUR 0x101 // Acknowledge moteur +#define ACKNOWLEDGE_IHM 0x102 // Acknowledge ecran tactile +#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) +#define ECRAN_START_MATCH 0x603 // Match (Indique que l'on souhaite commencer le match) +#define ECRAN_DEMO_BEGIN 0x604 // Demo (Indique que l'on souhaite faire une demo) +#define ECRAN_ACK_STRAT 0x611 // Acknowledge stratégie (si 0 erreur, sinon n°strat) +#define ECRAN_ACK_COLOR 0x612 // Acknowledge couleur (0->Blue;1->Yellow) +#define ECRAN_ACK_START_MATCH 0x613 // Acknowledge Match (Indique que l'on a bien reçu le debut du match) +#define ECRAN_ACK_DEMO 0x614 +#define ECRAN_ALL_CHECK 0x620 // Carte all check (Si provient de carte strat => toutes les cartes sont en ligne, Si provient IHM => forcer le lancement) +#define ECRAN_TIME 0x621 // Time match (Indication de moment cle du temps (10,30,60,70,80,85,90)) +#define ECRAN_PRINTF_1 0x6C0 // Tactile printf (Afficher les 8 permier caractères) +#define ECRAN_PRINTF_2 0x6C1 // Tactile printf (Afficher les 8 second caractères) +#define ECRAN_PRINTF_3 0x6C2 // Tactile printf (Afficher les 8 troisième caractères) +#define ECRAN_PRINTF_4 0x6C3 // Tactile printf (Afficher les 8 quatrième caractères) +#define ECRAN_PRINTF_CLEAR 0x6CF // Tactile printf clear (Permet d'effacer l'ecran) +#define ECRAN_CHOICE_START_ACTION 0x604 // Tactile printf clear (Choisir si il faut lancer le test actionneur) +#define ECRAN_ACK_CHOICE_START_ACTION 0x605 // Tactile printf clear (Ack du test actionneur) + +#define ERROR_OVERFLOW_BALISE 0x040 // Overflow odométrie +#define ERROR_OVERFLOW_MOTEUR 0x041 // Overflow asservissement +#define ERROR_OVERFLOW_IHM 0x042 // Overflow balise +#define ERROR_OVERFLOW_STRAT 0x043 // Overflow stratégie +#define ERROR_BALISE 0x785 // Bug balise +#define ERROR_RTC 0x786 // Bug RTC +#define ERROR_MOTEUR 0x787 // Bug moteur +#define ERROR_TELEMETRIE 0x788 // Bug télémètre +#define ERROR_STRATEGIE 0x789 // Bug stratégie + +#define DEBUG_STRATEGIE_AUTOMATE 0x760 // Etat automate stratégie (Permet de savoir l'etat de l'automate) +#define DEBUG_FAKE_JAKE 0x761 // Fake jack (Permet d'outre passerr le JACk du robot) +#define DEBUG_ASSERV 0x762 // Info debug carte moteur + +#define SERVO_AX12_SETGOAL 0x090 // AX12 setGoal (Indiquer la nouvelle position de l'AX12 !! Ne bouge pas) +#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 TELEMETRE_RECHERCHE_COIN 0x301 +#define TELEMETRE_OBJET 0x302 +#define OBJET_SUR_TABLE 0x304 + +#define POMPE_PWM 0x9A // pwm des pompes (pwm entre 0 et 100) + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed May 10 09:10:26 2017 +0000
@@ -0,0 +1,237 @@
+#include "global.h"
+
+/****************************************************************************************/
+/* FUNCTION NAME: automate_process */
+/* DESCRIPTION : Automate de gestion de la stratégie du robot */
+/****************************************************************************************/
+void automate_process(void);
+
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx */
+/* DESCRIPTION : Fonction de traitement des messages CAN */
+/****************************************************************************************/
+void canProcessRx(void);
+
+/*********************************************************************************************************/
+/* FUNCTION NAME: SendRawId */
+/* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
+/*********************************************************************************************************/
+void SendRawId (unsigned short id);
+
+DigitalOut led(LED1);
+Serial pc(SERIAL_TX, SERIAL_RX);
+/*DigitalIn IO1(PB_7);
+DigitalIn IO2(PB_6);
+DigitalIn IO3(PF_1);
+DigitalIn IO4(PA_8);
+
+DigitalIn A_in1(PB_7);
+DigitalIn A_in2(PA_6);
+DigitalIn A_in3(PA_4);
+DigitalIn A_in4(PA_1);
+DigitalIn A_in5(PA_0);
+
+DigitalOut AX12(PA_9);
+*/
+
+AnalogIn cptGauche(PA_1);
+AnalogIn cptDroit(PA_0);
+DigitalIn pressionGauche(PA_8);
+DigitalIn pressionDroit(PB_7);
+
+
+
+
+CAN can1(PA_11,PA_12); // Rx&Tx pour le CAN
+CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en réception pour le CAN
+unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
+
+signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
+
+extern "C" void mbed_reset();//Pour pouvoir reset la carte
+
+E_GameEtat carte_etat = ETAT_ATTENTE;
+E_Couleur couleurSelect = BLEU;
+
+AX12 *myAX12;
+
+
+
+
+void canRx_ISR (void)
+{
+ if (can1.read(msgRxBuffer[FIFO_ecriture])) {
+ /*if(msgRxBuffer[FIFO_ecriture].id==RESET_ACTIONNEURS) mbed_reset();
+ else FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;*/
+ FIFO_ecriture=(FIFO_ecriture+1)%SIZE_FIFO;
+ }
+}
+int main() {
+ char truc = 0;
+
+ can1.frequency(1000000); // fréquence de travail 1Mbit/s
+ can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
+
+ CANMessage msgTx=CANMessage();
+
+ myAX12-> Set_Goal_speed(vitesse);
+ myAX12-> Set_Mode(0);
+
+ pc.printf("Hello World\n");
+ while(1) {
+
+ led = ((led == 1)?0:1);
+
+ automate_process();
+ canProcessRx();
+ //SendRawId(cpt);
+ wait(0.5);
+ }
+}
+
+
+
+
+int action;
+char localAX12 = AX12_POSIOTION_BASSE;
+/****************************************************************************************/
+/* FUNCTION NAME: automate_process */
+/* DESCRIPTION : Automate de gestion de la stratégie du robot */
+/****************************************************************************************/
+void automate_process(void){
+ SendRawId(carte_etat);
+ switch(carte_etat){
+ case ETAT_ATTENTE:
+ break;
+
+ case ETAT_TRAITEMENT_ACTION_AX12:
+ switch(action){
+ #define ACTION_TOURNER_MODULE_GAUCHE 0x10
+ #define ACTION_TOURNER_MODULE_DROIT 0x11
+ case ACTION_TOURNER_MODULE_GAUCHE:
+ carte_etat = ETAT_BAISSER_MAIN_GAUCHE;
+ break;
+ case ACTION_TOURNER_MODULE_DROIT:
+ carte_etat = ETAT_BAISSER_MAIN_DROITE;
+ break;
+ default:
+ carte_etat = ETAT_ATTENTE;
+ }
+ break;
+
+ case ETAT_BAISSER_MAIN_GAUCHE:
+ if(pressionGauche.read() == ENFONCER){
+ carte_etat = ETAT_CHECK_COULEUR;
+ } else{
+ //
+ localAX12 = AX12BaisserMainGauche(localAX12);
+ }
+ break;
+
+ case ETAT_BAISSER_MAIN_DROITE:
+ if(pressionDroit.read() == ENFONCER){
+ carte_etat = ETAT_CHECK_COULEUR;
+ }else{
+ //
+ localAX12 = AX12BaisserMainDroite(localAX12);
+ }
+ break;
+
+ case ETAT_CHECK_COULEUR:
+ //if (((unsigned short)cptGauche.read()&(unsigned short)cptDroit.read()) == 0){
+ if ((unsigned short)cptDroit.read() == 0){
+ carte_etat = ETAT_ATTENTE;
+ }else{
+ // on active les moteurs
+ localAX12 = AX12BaisserMainGauche(localAX12);
+ }
+ break;
+
+ case ETAT_TURBINE:
+ carte_etat = ETAT_ATTENTE;
+ turbineWritePWM(50);
+ break;
+
+ case ETAT_POMPES:
+ carte_etat = ETAT_ATTENTE;
+ pompeWritePWM(50);
+ break;
+
+
+
+ default:
+ pc.printf("etat non identifie");
+ }
+ }
+
+
+/*********************************************************************************************************/
+/* FUNCTION NAME: SendRawId */
+/* DESCRIPTION : Envoie un message sans donnée, c'est-à-dire contenant uniquement un ID, sur le bus CAN */
+/*********************************************************************************************************/
+void SendRawId (unsigned short id)
+{
+ CANMessage msgTx=CANMessage();
+ msgTx.id=id;
+ msgTx.len=0;
+ can1.write(msgTx);
+ wait_us(200);
+}
+
+
+/****************************************************************************************/
+/* FUNCTION NAME: canProcessRx */
+/* DESCRIPTION : Fonction de traitement des messages CAN */
+/****************************************************************************************/
+void canProcessRx(void){
+ static signed char FIFO_occupation=0,FIFO_max_occupation=0;
+ CANMessage msgTx=CANMessage();
+ FIFO_occupation=FIFO_ecriture-FIFO_lecture;
+ if(FIFO_occupation<0)
+ FIFO_occupation=FIFO_occupation+SIZE_FIFO;
+ if(FIFO_max_occupation<FIFO_occupation)
+ FIFO_max_occupation=FIFO_occupation;
+ if(FIFO_occupation!=0) {
+
+ switch(msgRxBuffer[FIFO_lecture].id) {
+ case CHECK_ACTIONNEURS:
+ SendRawId(ALIVE_ACTIONNEURS);
+ break;
+
+ case SERVO_AX12_ACTION :
+ carte_etat = ETAT_TRAITEMENT_ACTION_AX12;
+
+ action = msgRxBuffer[FIFO_lecture].data[0];
+ couleurSelect = (E_Couleur)msgRxBuffer[FIFO_lecture].data[1];
+ //ACK de reception des actions a effectuer
+ msgTx.id = ACKNOWLEDGE_AX12;
+ msgTx.len = 1;
+ msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
+ can1.write(msgTx);
+ break;
+
+ case POMPE_PWM:
+ carte_etat = ETAT_POMPES;
+
+ msgTx.id = ACKNOWLEDGE_POMPES;
+ msgTx.len = 1;
+ msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
+ can1.write(msgTx);
+ break;
+
+ #define TURBINE_PWM 0xa1
+ #define ACKNOWLEDGE_TURBINE 0x107
+ case TURBINE_PWM:
+ carte_etat = ETAT_TURBINE;
+
+ msgTx.id = ACKNOWLEDGE_TURBINE;
+ msgTx.len = 1;
+ msgTx.data[0] = msgRxBuffer[FIFO_lecture].data[0];
+ can1.write(msgTx);
+ break;
+
+ }
+ FIFO_lecture=(FIFO_lecture+1)%SIZE_FIFO;
+ }
+ }
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Wed May 10 09:10:26 2017 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/794e51388b66 \ No newline at end of file