Revision 41:b5a2fbc20beb, committed 2016-04-13

Comitter:

sype

Date:

Wed Apr 13 11:27:34 2016 +0000

Parent:

39:309f38d1e49c

Child:

42:d78d5dadf8a2

Commit message:

Impl?mentation des dispositifs du robot

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.cpp	Wed Apr 13 11:27:34 2016 +0000
@@ -0,0 +1,550 @@
+/* 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 "AX12.h"
+#include "mbed.h"
+
+extern Serial logger;
+
+AX12::AX12(PinName tx, PinName rx, int ID, int baud)
+        : _ax12(tx,rx) {
+    _baud = baud;
+    _ID = ID;
+    _ax12.baud(_baud);
+
+}
+
+// set the mode of the servo
+//  0 = Positional (0-300 degrees)
+//  1 = Rotational -1 to 1 speed
+int AX12::setMode(int mode) {
+
+    if (mode == 1) { // set CR
+        setCWLimit(0);
+        setCCWLimit(0);
+        setCRSpeed(0.0);
+    } else {
+        setCWLimit(0);
+        setCCWLimit(300);
+        setCRSpeed(0.0);
+    }
+    return(0);
+}
+
+
+// if flag[0] is set, were blocking
+// if flag[1] is set, we're registering
+// they are mutually exclusive operations
+int AX12::setGoal(int degrees, int flags) {
+
+    char reg_flag = 0;
+    char data[2];
+    _goal = degrees;
+
+    // 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
+    logger.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);
+}
+
+int AX12::setMaxTorque(int maxTorque)
+{
+    char data[2];
+    
+    data[0] = maxTorque & 0xFF;
+    data[1] = maxTorque >> 8;
+    
+    // write the packet, return the error code
+    int rVal = write(_ID, 0x22, 2, data);
+
+    return(rVal);
+}
+
+
+// set continuous rotation speed from -1 to 1
+int AX12::setCRSpeed(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::setCWLimit (int degrees) {
+
+    char data[2];
+
+    // 1023 / 300 * degrees
+    short limit = (1023 * degrees) / 300;
+
+#ifdef AX12_DEBUG
+    logger.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::setCCWLimit (int degrees) {
+
+    char data[2];
+
+    // 1023 / 300 * degrees
+    short limit = (1023 * degrees) / 300;
+
+#ifdef AX12_DEBUG
+    logger.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::setID (int CurrentID, int NewID) {
+
+    char data[1];
+    data[0] = NewID;
+
+#ifdef AX12_DEBUG
+    logger.printf("setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
+#endif
+
+    return (write(CurrentID, AX12_REG_ID, 1, data));
+
+}
+
+
+int AX12::setBaud (int baud) {
+
+    char data[1];
+    data[0] = baud;
+
+#ifdef AX12_DEBUG
+    logger.printf("setting Baud rate to %d\n",baud);
+#endif
+
+    return (write(0xFE, 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::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
+    logger.printf("\nTriggered\n");
+    logger.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
+    logger.printf("  ID : %d\n",TxBuf[2]);
+#endif
+
+    // Length
+    TxBuf[3] = 0x02;
+    sum += TxBuf[3];
+
+#ifdef AX12_TRIGGER_DEBUG
+    logger.printf("  Length %d\n",TxBuf[3]);
+#endif
+
+    // Instruction - ACTION
+    TxBuf[4] = 0x04;
+    sum += TxBuf[4];
+
+#ifdef AX12_TRIGGER_DEBUG
+    logger.printf("  Instruction 0x%X\n",TxBuf[5]);
+#endif
+
+    // Checksum
+    TxBuf[5] = 0xFF - sum;
+#ifdef AX12_TRIGGER_DEBUG
+    logger.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::getPosition(void) {
+
+#ifdef AX12_DEBUG
+    logger.printf("\ngetPosition(%d)",_ID);
+#endif
+
+    char data[2];
+
+    int ErrorCode = read(_ID, AX12_REG_POSITION, 2, data);
+    short position = data[0] + (data[1] << 8);
+    float angle = (position * 300)/1024;
+
+    return (angle);
+}
+
+
+float AX12::getTemp (void) {
+
+#ifdef AX12_DEBUG
+    logger.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::getVolts (void) {
+
+#ifdef AX12_DEBUG
+    logger.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 = 0x4;
+    char TxBuf[16];
+    char sum = 0;
+    char Status[16];
+
+    Status[4] = 0xFE; // return code
+
+#ifdef AX12_READ_DEBUG
+    logger.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
+    logger.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
+    logger.printf("  ID : %d\n",TxBuf[2]);
+#endif
+
+    // Packet Length
+    TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
+    sum += TxBuf[3];            // Accululate the packet sum
+
+#ifdef AX12_READ_DEBUG
+    logger.printf("  Length : 0x%x\n",TxBuf[3]);
+#endif
+
+    // Instruction - Read
+    TxBuf[4] = 0x2;
+    sum += TxBuf[4];
+
+#ifdef AX12_READ_DEBUG
+    logger.printf("  Instruction : 0x%x\n",TxBuf[4]);
+#endif
+
+    // Start Address
+    TxBuf[5] = start;
+    sum += TxBuf[5];
+
+#ifdef AX12_READ_DEBUG
+    logger.printf("  Start Address : 0x%x\n",TxBuf[5]);
+#endif
+
+    // Bytes to read
+    TxBuf[6] = bytes;
+    sum += TxBuf[6];
+
+#ifdef AX12_READ_DEBUG
+    logger.printf("  No bytes : 0x%x\n",TxBuf[6]);
+#endif
+
+    // Checksum
+    TxBuf[7] = 0xFF - sum;
+#ifdef AX12_READ_DEBUG
+    logger.printf("  Checksum : 0x%x\n",TxBuf[7]);
+#endif
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i<8 ; i++) {
+        _ax12.putc(TxBuf[i]);
+    }
+
+    // Wait for the bytes to be transmitted
+    wait (0.00002);
+
+    // Skip if the read was to the broadcast address
+    if (_ID != 0xFE) {
+
+
+
+        // response packet is always 6 + bytes
+        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
+        // timeout is a little more than the time to transmit
+        // the packet back, i.e. (6+bytes)*10 bit periods
+
+        int timeout = 0;
+        int plen = 0;
+        while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {
+
+            if (_ax12.readable()) {
+                Status[plen] = _ax12.getc();
+                plen++;
+                timeout = 0;
+            }
+
+            // wait for the bit period
+            wait (1.0/_baud);
+            timeout++;
+        }
+
+        if (timeout == ((6+bytes)*10) ) {
+            return(-1);
+        }
+
+        // Copy the data from Status into data for return
+        for (int i=0; i < Status[3]-2 ; i++) {
+            data[i] = Status[5+i];
+        }
+
+#ifdef AX12_READ_DEBUG
+        logger.printf("\nStatus Packet\n");
+        logger.printf("  Header : 0x%x\n",Status[0]);
+        logger.printf("  Header : 0x%x\n",Status[1]);
+        logger.printf("  ID : 0x%x\n",Status[2]);
+        logger.printf("  Length : 0x%x\n",Status[3]);
+        logger.printf("  Error Code : 0x%x\n",Status[4]);
+
+        for (int i=0; i < Status[3]-2 ; i++) {
+            logger.printf("  Data : 0x%x\n",Status[5+i]);
+        }
+
+        logger.printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
+#endif
+
+    } // if (ID!=0xFE)
+
+    return(Status[4]);
+}
+
+
+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];
+
+#ifdef AX12_WRITE_DEBUG
+    logger.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
+    logger.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
+    logger.printf("  ID : %d\n",TxBuf[2]);
+#endif
+
+    // packet Length
+    TxBuf[3] = 3+bytes;
+    sum += TxBuf[3];
+
+#ifdef AX12_WRITE_DEBUG
+    logger.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
+    logger.printf("  Instruction : 0x%x\n",TxBuf[4]);
+#endif
+
+    // Start Address
+    TxBuf[5] = start;
+    sum += TxBuf[5];
+
+#ifdef AX12_WRITE_DEBUG
+    logger.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
+        logger.printf("  Data : 0x%x\n",TxBuf[6+i]);
+#endif
+
+    }
+
+    // checksum
+    TxBuf[6+bytes] = 0xFF - sum;
+
+#ifdef AX12_WRITE_DEBUG
+    logger.printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
+#endif
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i < (7 + bytes) ; i++) {
+        _ax12.putc(TxBuf[i]);
+    }
+
+    // Wait for data to transmit
+    wait (0.00002);
+
+    // make sure we have a valid return
+    Status[4]=0x00;
+
+    // we'll only get a reply if it was not broadcast
+    if (_ID!=0xFE) {
+
+
+        // response packet is always 6 bytes
+        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
+        // timeout is a little more than the time to transmit
+        // the packet back, i.e. 60 bit periods, round up to 100
+        int timeout = 0;
+        int plen = 0;
+        while ((timeout < 100) && (plen<6)) {
+
+            if (_ax12.readable()) {
+                Status[plen] = _ax12.getc();
+                plen++;
+                timeout = 0;
+            }
+
+            // wait for the bit period
+            wait (1.0/_baud);
+            timeout++;
+        }
+
+
+        // Build the TxPacket first in RAM, then we'll send in one go
+#ifdef AX12_WRITE_DEBUG
+        logger.printf("\nStatus Packet\n  Header : 0x%X, 0x%X\n",Status[0],Status[1]);
+        logger.printf("  ID : %d\n",Status[2]);
+        logger.printf("  Length : %d\n",Status[3]);
+        logger.printf("  Error : 0x%x\n",Status[4]);
+        logger.printf("  Checksum : 0x%x\n",Status[5]);
+#endif
+
+
+    }
+
+    return(Status[4]); // return error code
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.h	Wed Apr 13 11:27:34 2016 +0000
@@ -0,0 +1,203 @@
+/* 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 "mbed.h"
+
+//#define AX12_WRITE_DEBUG 0
+//#define AX12_READ_DEBUG 0
+//#define AX12_TRIGGER_DEBUG 0
+//#define AX12_DEBUG 0
+
+#define AX12_REG_ID 0x3
+#define AX12_REG_BAUD 0x4
+#define AX12_REG_CW_LIMIT 0x06
+#define AX12_REG_CCW_LIMIT 0x08
+#define AX12_REG_GOAL_POSITION 0x1E
+#define AX12_REG_MOVING_SPEED 0x20
+#define AX12_REG_VOLTS 0x2A
+#define AX12_REG_TEMP 0x2B
+#define AX12_REG_MOVING 0x2E
+#define AX12_REG_POSITION 0x24
+
+#define AX12_MODE_POSITION  0
+#define AX12_MODE_ROTATION  1
+
+#define AX12_CW 1
+#define AX12_CCW 0
+
+#/** 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);
+
+    /** Set the mode of the servo
+     * @param mode
+     *    0 = Positional, default
+     *    1 = Continuous rotation
+     */
+    int setMode(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 setBaud(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 setGoal(int degrees, int flags = 0);
+    
+    /** Get goal angle (internal, not from the AX12)
+     *
+     */
+     
+     int getGoal(){return _goal;}
+    
+    
+    /** Set the torque limit of the servo
+     *
+     * @param maxTorque, 0-1024
+     */
+    int setMaxTorque(int maxTorque);
+
+
+    /** 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 setCRSpeed(float speed);
+
+
+    /** Set the clockwise limit of the servo
+     *
+     * @param degrees, 0-300
+     */
+    int setCWLimit(int degrees);
+    
+    /** Set the counter-clockwise limit of the servo
+     *
+     * @param degrees, 0-300
+     */
+    int setCCWLimit(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 setID(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);
+
+    /** Read the current angle of the servo
+     *
+     * @returns float in the range 0.0-300.0
+     */
+    float getPosition();
+
+    /** Read the temperature of the servo
+     *
+     * @returns float temperature 
+     */
+    float getTemp(void);
+
+    /** Read the supply voltage of the servo
+     *
+     * @returns float voltage
+     */
+    float getVolts(void);
+
+    int read(int ID, int start, int length, char* data);
+    int write(int ID, int start, int length, char* data, int flag=0);
+
+private :
+
+    Serial _ax12;
+    int _ID;
+    int _baud;
+    int _goal;
+
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Functions/func.cpp	Wed Apr 13 11:27:34 2016 +0000
@@ -0,0 +1,134 @@
+#include "func.h"
+
+void pressed(void)
+{
+    if(i==0) {
+        roboclaw.ForwardM1(ADR, 0);
+        roboclaw.ForwardM2(ADR, 0);
+        i++;
+    }
+}
+
+void unpressed(void)
+{
+    if(i==1) {
+        i--;
+    }
+}
+
+void ELpressed(void)
+{
+    //bleu = 1;
+    EL = true;
+}
+void ELunpressed(void)
+{
+    //bleu = 0;
+    EL = false;
+}
+
+void EZpressed(void)
+{
+    //blanc = 1;
+    EZ = true;
+}
+void EZunpressed(void)
+{
+    //blanc = 0;
+    EZ = false;
+}
+
+void ERpressed(void)
+{
+    //rouge = 1;
+    ER = true;
+}
+void ERunpressed(void)
+{
+    //rouge = 0;
+    ER = 0;
+}
+
+void JPO(void)
+{
+    char c = logger.getc();
+    if(c=='z') {
+        if (state != 1) {
+            state = 1;
+            logger.printf("Avant (Z) \r\n");
+            roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+        }
+    } else if(c == 's') {
+        if (state != 2) {
+            state = 2;
+            logger.printf("Stop (S) \r\n");
+            roboclaw.SpeedAccelM1(ADR, accel_angle, 0);
+            roboclaw.SpeedAccelM2(ADR, accel_angle, 0);
+        }
+    } else if(c == 'd') {
+        if (state != 3) {
+            state = 3;
+            logger.printf("Droite (D) \r\n");
+            roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
+            roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
+        }
+    } else if(c == 'q') {
+        if (state != 4) {
+            state = 4;
+            logger.printf("Gauche (Q)\r\n");
+            roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
+            roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
+        }
+    } else if(c == 'x') {
+        if (state != 5) {
+            state = 5;
+            roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
+            roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
+        }
+    } else {
+        if (state != 0) {
+            roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
+            state = 0;
+        }
+    }
+}
+
+void goHome(void)
+{
+    while(EZ==false) ZMot.step(1, 0, DELAY);
+    while(ER==false) RMot.step(1, 0, DELAY);
+    RMot.step(10, 1, DELAY);
+    RMot.step(5, 0, DELAY);
+    while(EL==false) LMot.step(1, 0, DELAY);
+    LMot.step(10, 1, DELAY);
+    LMot.step(5, 0, DELAY);
+}
+
+void checkAround(void)
+{
+    if(capt1 > SEUIL+0.1) bleu = 1;
+    else bleu = 0;
+    if(capt2 > SEUIL) blanc = 1;
+    else blanc = 0;
+    if(capt3 > SEUIL+0.1) rouge = 1;
+    else rouge = 0;
+}
+
+void init_ax12(void)
+{
+    left_hand.setMode(0);
+    wait_ms(10);
+    right_hand.setMode(0);
+    wait_ms(50);
+    left_hand.setMode(0);
+    wait_ms(10);
+    right_hand.setMode(0);
+    wait_ms(50);
+    right_hand.setGoal(0);
+    left_hand.setGoal(0);
+    wait(2);
+    right_hand.setGoal(180);
+    left_hand.setGoal(180);
+    wait(2);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Functions/func.h	Wed Apr 13 11:27:34 2016 +0000
@@ -0,0 +1,43 @@
+#ifndef FUNC_H
+#define FUNC_H
+
+#include "mbed.h"
+#include "../RoboClaw/RoboClaw.h"
+#include "../Odometry/Odometry.h"
+#include "../StepperMotor/Stepper.h"
+#include "Map/Map.h"
+#include "AX12.h"
+
+#define SEUIL 0.25
+
+extern Serial logger;
+extern RoboClaw roboclaw;
+extern DigitalOut bleu;
+extern DigitalOut blanc;
+extern DigitalOut rouge;
+extern Stepper RMot;
+extern Stepper ZMot;
+extern Stepper LMot;
+extern AnalogIn capt1;
+extern AnalogIn capt2;
+extern AnalogIn capt3;
+
+extern int i, state;
+extern bool EL, EZ, ER;
+
+void ELpressed(void);
+void ELunpressed(void);
+void EZpressed(void);
+void EZunpressed(void);
+void ERpressed(void);
+void ERunpressed(void);
+
+void pressed(void);
+void unpressed(void);
+void JPO(void);
+
+void init_ax12(void);
+void goHome(void);
+void checkAround(void);
+
+#endif
\ No newline at end of file

--- a/Map/Map.cpp	Tue Apr 05 15:02:12 2016 +0000
+++ b/Map/Map.cpp	Wed Apr 13 11:27:34 2016 +0000
@@ -1,5 +1,5 @@
 #include "Map.h"
-
+#include "../Odometry/Odometry.h"
 #include "Obstacles/Obs_circle.h"
 
 #ifdef CODEBLOCK

--- a/Map/defines.h	Tue Apr 05 15:02:12 2016 +0000
+++ b/Map/defines.h	Wed Apr 13 11:27:34 2016 +0000
@@ -80,8 +80,6 @@
 #define ODO_D_B PB_5
 #define ODO_D_A PB_4
 
-#define PI 3.14159f
-
 // ----- Boutons ----- //
 
 #define LED_DESSUS PH_1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/StepperMotor.lib	Wed Apr 13 11:27:34 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/melse/code/StepperMotor/#1cec7e9117f2

--- a/main.cpp	Tue Apr 05 15:02:12 2016 +0000
+++ b/main.cpp	Wed Apr 13 11:27:34 2016 +0000
@@ -1,137 +1,99 @@
-#include "Odometry/Odometry.h"
-#include "Map/Map.h"
+#include "func.h"
 
 #define dt 10000
 #define ATTENTE 0
 #define GO 1
 #define STOP 2
 
+/* Déclaration des différents éléments de l'IHM */
 InterruptIn mybutton(USER_BUTTON);
 DigitalIn button(USER_BUTTON);
 DigitalOut led(LED1);
+DigitalOut bleu(PC_5);
+DigitalOut blanc(PC_6);
+DigitalOut rouge(PC_8);
+
+/* AX12 */
+AX12 left_hand(PA_15, PB_7, 4, 250000);
+AX12 right_hand(PA_15, PB_7, 1, 250000);
+
+/* Sharp */
+AnalogIn capt1(PA_4);
+AnalogIn capt2(PB_0);
+AnalogIn capt3(PC_1);
+AnalogIn capt4(PC_0);
+
+/* Moteurs pas à pas */
+Stepper RMot(NC, PB_10, PA_8);
+Stepper ZMot(NC, PB_5, PB_4);
+Stepper LMot(NC, PB_3, PA_10);
+/* Fins de course */
+InterruptIn EndR(PB_15);
+InterruptIn EndZ(PB_14);
+InterruptIn EndL(PB_13);
+
 Ticker ticker;
-//Serial pc(USBTX, USBRX);
-Serial logger (PA_9, PA_10);
+
+Serial logger(USBTX, USBRX);
+//Serial logger(PA_9, PA_10);
 RoboClaw roboclaw(460800, PA_11, PA_12);
 Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);
 
-int i = 0;
+int i = 0, state = 0;
+bool EL = false, EZ = false, ER = false;
 
 void update_main(void);
 void init(void);
-void pressed(void);
-void unpressed(void);
 
 /** Debut du programme */
 int main(void)
 {
     init();
     /* Code AStar */
-    
-    double angle_v = 2*PI/5;
+
+    /*double angle_v = 2*PI/5;
     double distance_v = 200.0;
     std::vector<SimplePoint> path;
     Map map;
-    
-    
+
+
     //Construction des obstacles
     map.build();
-    
+
     float x=odo.getX();
     float y=odo.getY();
     float the = 0;
-    
+
     map.AStar(x, y, 1600, 1000, 75);
     path = map.path;
-    
+
     for(int i=0; i<path.size();i++) {
-        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
-        odo.GotoXYT(path[i].x, path[i].y, the);
-    }
-    
-    map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
-    path = map.path;
-    
-    for(int i=0; i<path.size();i++) {
-        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
+        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
         odo.GotoXYT(path[i].x, path[i].y, the);
     }
-    
+
+    map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
+    path = map.path;
+
+    for(int i=0; i<path.size();i++) {
+        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
+        odo.GotoXYT(path[i].x, path[i].y, the);
+    }*/
+
     //odo.GotoThet(PI);
-    odo.GotoThet(0);
+    //odo.GotoThet(0);
     //odo.TestEntraxe(3);
-    
+
     //odo.GotoThet(-PI/2);
-    wait(2000);
+    //wait(2000);
     //odo.GotoXYT(2250,500,0);
-    
+
     //odo.TestEntraxe(5);
     //odo.Forward(1000);
-    
-    /* Code JPO :
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
-    int state = 0;
     while(1)
     {
-       // while(logger.readable()) 
-        //{
-            char c = logger.getc();
-            if(c=='z')
-            {
-                if (state != 1) {
-                    state = 1;
-                    logger.printf("Avant (Z) \r\n");
-                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
-                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
-                }
-            }
-            else if(c == 's')
-            {
-                if (state != 2) {
-                    state = 2;
-                    logger.printf("Stop (S) \r\n");
-                    roboclaw.ForwardM1(ADR, 0);
-                    roboclaw.ForwardM2(ADR, 0);
-                }
-            }
-            else if(c == 'd')
-            {
-                if (state != 3) {
-                    state = 3;
-                    logger.printf("Droite (D) \r\n");
-                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
-                    roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
-                }
-            }
-            else if(c == 'q')
-            {
-                if (state != 4) {
-                    state = 4;
-                    logger.printf("Gauche (Q)\r\n");
-                    roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
-                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
-                }
-            }
-            else if(c == 'x')
-            {
-                if (state != 5) {
-                    state = 5;
-                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, -vitesse_angle, accel_angle, -vitesse_angle);
-                }
-            }
-            else if (c == '\0') { ; }
-            else {
-                if (state != 0) {
-                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
-                    state = 0;
-                }
-            }
-       // }
-       // roboclaw.ForwardM1(ADR, 0);
-       // roboclaw.ForwardM2(ADR, 0);
-       
-    }*/
+        JPO();
+    }
 }
 
 void init(void)
@@ -141,38 +103,35 @@
     logger.baud(9600);
     logger.printf("Hello from main !\n\r");
     wait_ms(500);
+    bleu = 1;
+    blanc = 1;
+    rouge = 1;
+    wait_ms(1000);
+    bleu = 0;
+    blanc = 0;
+    rouge = 0;
     
-    odo.setPos(0, 1000, 0);
-        
+    //odo.setPos(0, 1000, 0);
     while(button);
     wait(1);
+
     mybutton.fall(&pressed);
     mybutton.rise(&unpressed);
+
+    EndL.fall(&ELpressed);
+    EndL.rise(&ELunpressed);
+    EndZ.fall(&EZpressed);
+    EndZ.rise(&EZunpressed);
+    EndR.fall(&ERpressed);
+    EndR.rise(&ERunpressed);
+
+    wait_ms(100);
     ticker.attach_us(&update_main,dt); // 100 Hz
-    
     logger.printf("End init\n\r");
 }
 
 void update_main(void)
 {
     odo.update_odo();
-    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
-    //if(pc.readable()) if(pc.getc()=='l') led = !led;
-}
-
-void pressed(void)
-{
-    if(i==0) {
-        roboclaw.ForwardM1(ADR, 0);
-        roboclaw.ForwardM2(ADR, 0);
-        //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
-        i++;
-    }
-}
-
-void unpressed(void)
-{
-    if(i==1) {
-        i--;
-    }
+    checkAround();
 }
\ No newline at end of file