Revision 80:cd4960dfa47e, committed 2016-05-05

Comitter:

IceTeam

Date:

Thu May 05 16:01:55 2016 +0000

Parent:

79:b11b50108ae5

Child:

81:e7b03e81b025

Commit message:

Yo !;

Changed in this revision

--- a/AX12/AX12.cpp	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,557 +0,0 @@
-/* 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);
-}
-
-float AX12::getLoad (void) {
-    char data[2];
-    int ErrorCode = read(_ID, AX12_REG_LOAD, 2, data);
-    short load1 = data[0] + (data[1] << 8);
-    float load = load1/1023;
-    return(load); //renvoie la charge en % 
-}
-
-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
-}

--- a/AX12/AX12.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,204 +0,0 @@
-/* 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_REG_LOAD 0x28
-
-#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);
-    float getLoad(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

--- a/Functions/DefinesSharps.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,6 +0,0 @@
-#ifndef DEFINES_SHARPS_H
-#define DEFINES_SHARPS_H
-
-//extern bool SGauche, SDevant, SDroite, SHomologation;
-
-#endif
\ No newline at end of file

--- a/Functions/defines.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,58 +0,0 @@
-#ifndef DEFINES_H
-#define DEFINES_H
-
-#include "DefinesSharps.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
-#define VERT 1
-#define VIOLET 2
-#define ATTENTE 0
-#define GO 1
-#define STOP 2
-#define ADR 0x80
-#define dt 10000
-#define HAUT 1
-#define BAS 0
-
-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 InterruptIn mybutton;
-extern InterruptIn EndR;
-extern InterruptIn EndZ;
-extern InterruptIn EndL;
-//extern AX12 left_hand;
-//extern AX12 right_hand;
-extern Odometry odo;
-extern Ticker ticker;
-extern DigitalIn CAMP;
-extern DigitalIn START;
-extern DigitalOut LEDR;
-extern DigitalOut LEDV;
-extern DigitalIn button;
-extern BusOut drapeau;
-extern DigitalIn EnR;
-extern DigitalIn EnZ;
-extern DigitalIn EnL;
-extern float seuils[3];
-extern int SGauche, SDevant, SDroite;
-
-
-extern int SIMON_i, SIMON_state, SCouleur, SStart, SSchema;
-extern bool SIMON_EL, SIMON_EZ, SIMON_ER;
-
-#endif
\ No newline at end of file

--- a/Functions/func.cpp	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,115 +0,0 @@
-#include <cmath>
-
-#include "func.h"
-
-int max(int a, int b)
-{
-    return (a>b)?a:b;
-}
-
-int max(int a, int b, int c)
-{
-    return max(a,max(b,c));
-}
-
-int min(int a, int b)
-{
-    return (a<b)?a:b;
-}
-
-void init_interrupt(void)
-{
-    ticker.attach_us(&update_main, dt); // 100 Hz
-}
-
-void pressed(void)
-{
-    if(SIMON_i==0) {
-        roboclaw.ForwardM1(0);
-        roboclaw.ForwardM2(0);
-        SIMON_i++;
-    }
-}
-
-void unpressed(void)
-{
-    if(SIMON_i==1) {
-        SIMON_i--;
-    }
-}
-
-void checkAround(void)
-{
-    SDroite += (capt1.read() > seuils[0]?1:-1);
-    SDroite = min(SDroite, 5);
-    SDroite = max(SDroite, -5);
-    
-    SDevant += (capt2.read() > seuils[1]?1:-1);
-    SDevant = min(SDevant, 5);
-    SDevant = max(SDevant, -5);
-    
-    SGauche += (capt3.read() > seuils[2]?1:-1);
-    SGauche = min(SGauche, 5);
-    SGauche = max(SGauche, -5);
-    
-    drapeau[0] = SGauche>0;
-    drapeau[1] = SDevant>0;
-    drapeau[2] = SDroite>0;
-    
-    logger.printf("Sharps : %d %d %d\r\n", SGauche, SDevant, SDroite);
-}
-
-/*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);
-}*/
-
-void changeCamp(void)
-{
-    if(SCouleur==VERT) {
-        SCouleur = VIOLET;
-        LEDR = 1;
-        LEDV = 0;
-    } else {
-        SCouleur = VERT;
-        LEDV = 1;
-        LEDR = 0;
-    }
-}
-
-void depart(void)
-{
-    while(button==1) {
-        if(CAMP==0) {
-            changeCamp();
-        }
-        wait_ms(100);
-    }
-
-    if(SCouleur == VERT)logger.printf("Couleur VERT !\n\r");
-    else logger.printf("Couleur ROUGE !\n\r");
-
-    while(START==0) {
-        drapeau = SSchema;
-        if(CAMP==0) {
-            SSchema++;
-            if(SSchema == 6) SSchema = 1;
-            drapeau = SSchema;
-        }
-        wait_ms(100);
-    }
-    logger.printf("Schema numero : %d !\n\r", SSchema);
-}

--- a/Functions/func.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,16 +0,0 @@
-#include "defines.h"
-
-void depart(void);
-void changeCamp(void);
-
-void pressed(void);
-void unpressed(void);
-
-void init_ax12(void);
-void init_interrupt(void);
-void checkAround(void);
-void update_main(void);
-
-int max(int a, int b);
-
-int max(int a, int b, int c);
\ No newline at end of file

--- a/Map/controle.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,17 +0,0 @@
-#ifndef CONTROLE_H
-#define CONTROLE_H
-
-/* Points du rectangle :
-    0       1
-
-    2       3
-
-    Penser à supprimer MINDISTROBOT et le mettre dans la taille de l'obstacle (on augmentera dxtaille et dytaille de façon adaptée)
-    Autre methode d'amelioration : dans la boucle de test de croisement des obstacles, s'il n'y a pas de croisement, mettre ended=true, ca évitera de faire les tests pour tous les points disponibles !
-*/
-
-#define NULL 0
-#define BATARD 0xCCCCCCCC
-#define MINDISTROBOT 1
-
-#endif

--- a/Map/figure.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,41 +0,0 @@
-#ifndef FIGURE_H
-#define FIGURE_H
-
-#include "point.h"
-
-class figure {
-public:
-    figure(float xc, float yc) {
-        xcentre = xc;
-        ycentre = yc;
-    }
-
-    float getX () { return xcentre; }
-    float getY () { return ycentre; }
-
-    bool CroisementSegment (point A1, point A2, point B1, point B2) {
-        if (ProdVect (A1, A2, B1, B2) != 0) { // On verifie que les droites ne sont pas parallèles
-            if (ProdVect (A1, A2, A1, B2)*ProdVect (A1, A2, A1, B1) <= 0
-                && ProdVect (B1, B2, B1, A1)*ProdVect (B1, B2, B1, A2) <= 0) {
-                return true;
-            }
-            else {
-                return false;
-            }
-        }
-        else {
-            return false;
-        }
-    }
-
-protected:
-    float xcentre, ycentre;
-
-    float ProdVect (float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4) {
-        return ((x2 - x1)*(y4 - y3) - (y2 - y1)*(x4 - x3));
-    }
-    float ProdVect (point A1, point A2, point B1, point B2) {
-        return ProdVect (A1.getX(), A1.getY (), A2.getX (), A2.getY (), B1.getX (), B1.getY (), B2.getX (), B2.getY ());
-    }
-};
-#endif
\ No newline at end of file

--- a/Map/map.cpp	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,159 +0,0 @@
-#include "map.h"
-
-map::map (Odometry* nodo) : Codo(nodo) {
-}
-
-void map::addObs (obsCarr nobs) {
-    obs.push_back (nobs);
-}
-
-void map::FindWay (float depX, float depY, float arrX, float arrY) {
-    point depart(depX, depY);
-    point arrivee(arrX, arrY);
-    FindWay(depart, arrivee);
-}
-
-void map::FindWay (point dep, point arr) {
-    //logger.printf("On a cherche un chemin\n\r");
-    nVector<pointParcours> open;
-    nVector<pointParcours> close;
-    points4 tmp;
-    bool val[4] = {true,true,true,true};
-    int os = obs.size ();
-    int i, j;
-    bool ended=false;   // On teste tous les points ajoutes dans l'open list pour savoir s'il y a intersection avec un obstacle. Ended passe à true quand aucun ne coupe un obstacle.
-    endedParc = false;
-
-    path.clear();
-    
-    pointParcours depP (dep, NULL, arr);
-    int indTMP1=0;  // Le point actuel
-    int PointEnding = 0;    
-    open.push_back (depP);
-
-    while (!ended && !open.empty ()) {
-        for (i = 0; i < open.size (); ++i) {
-            if (open[i].getP2 () < open[indTMP1].getP2 ())
-                indTMP1 = i;
-        }
-
-        close.push_first (open[indTMP1]);
-        open.erase (indTMP1);
-        indTMP1 = 0;
-
-        ended = true;
-        for (i = 0; i < os; ++i) {
-            if (obs[i].getCroisement (close[indTMP1].getX (), close[indTMP1].getY (), arr)) {
-                ended = false;
-                tmp = obs[i].getPoints ();
-
-                // On vérifie si le point croise un obstacle
-                for (j = 0; j < os; ++j)
-                    if (obs[j].getCroisement (tmp.p0, close[indTMP1]))
-                        val[0] = false;
-                // On vérifie si le point existe déjà dans la liste ouverte
-                for (j = 0; j < open.size (); ++j) {
-                    if (open[j] == tmp.p0)
-                        val[0] = false;
-                }
-                // On vérifie si le point existe déjà dans la liste fermée
-                for (j = 0; j < close.size (); ++j) {
-                    if (close[j] == tmp.p0)
-                        val[0] = false;
-                }
-                if (val[0]) {
-                    open.push_back (pointParcours (tmp.p0, &close[indTMP1], arr));
-                }
-
-                // On repete l'operation pour le second point
-                for (j = 0; j < os; ++j)
-                    if (obs[j].getCroisement (tmp.p1, close[indTMP1]))
-                        val[1] = false;
-                for (j = 0; j < open.size (); ++j) {
-                    if (open[j] == tmp.p1)
-                        val[1] = false;
-                }
-                for (j = 0; j < close.size (); ++j) {
-                    if (close[j] == tmp.p1)
-                        val[1] = false;
-                }
-                if (val[1]) {
-                    open.push_back (pointParcours (tmp.p1, &close[indTMP1], arr));
-                }
-
-                // On répète l'opération pour le troisième point
-                for (j = 0; j < os; ++j)
-                    if (obs[j].getCroisement (tmp.p2, close[indTMP1]))
-                        val[2] = false;
-                for (j = 0; j < open.size (); ++j) {
-                    if (open[j] == tmp.p2)
-                        val[2] = false;
-                }
-                for (j = 0; j < close.size (); ++j) {
-                    if (close[j] == tmp.p2)
-                        val[2] = false;
-                }
-                if (val[2]) {
-                    open.push_back (pointParcours (tmp.p2, &close[indTMP1], arr));
-                }
-
-                // On répète l'opération pour le quatrieme point
-                for (j = 0; j < os; ++j)
-                    if (obs[j].getCroisement (tmp.p3, close[indTMP1]))
-                        val[3] = false;
-                for (j = 0; j < open.size (); ++j) {
-                    if (open[j] == tmp.p3)
-                        val[3] = false;
-                }
-                for (j = 0; j < close.size (); ++j) {
-                    if (close[j] == tmp.p3)
-                        val[3] = false;
-                }
-                if (val[3]) {
-                    open.push_back (pointParcours (tmp.p3, &close[indTMP1], arr));
-                }
-
-                val[0] = true;
-                val[1] = true;
-                val[2] = true;
-                val[3] = true;
-            }
-        }
-    }
-
-    /* L'algorithme n'est pas bon. Je devrais prendre ici le plus court chemin vers l'arrivée pour ceux qui ne sont pas bloqués, et pas un aléatoire ... */
-    if (ended) {
-        pointParcours* pente;
-        pente = &close[0];
-        while (pente != NULL) {
-            path.push_first (*pente);
-            pente = pente->getPere ();
-        }
-        path.push_back (pointParcours(arr, NULL, arr));
-        path.erase(0);
-        endedParc = true;
-        /*
-        if (path.size() > 1)
-            path.erase(0);*/
-    }
-}
-
-void map::Execute(float XObjectif, float YObjectif) {
-    logger.printf("Findway %f-%f -> %f-%f\n\r", Codo->getX(), Codo->getY(), XObjectif, YObjectif);
-    FindWay (Codo->getX(), Codo->getY(), XObjectif, YObjectif);
-    
-    if (endedParc) {
-        //logger.printf("\n\r");
-        for (int i = 0; i < path.size (); i++) {
-            logger.printf("Goto %d/%d [%f, %f]... \n\r", i, path.size()-1, path[i].getX(), path[i].getY());
-            //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX())); 
-            Codo->GotoXY((double)path[i].getX(), (double)path[i].getY());
-            logger.printf("Goto Fini\n\r");
-        }
-        //logger.printf("Chemin fini !\n\r");
-    }
-    else {
-        logger.printf("Chemin pas trouve ...\n\r");
-    }
-    endedParc = false;
-}
\ No newline at end of file

--- a/Map/map.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,27 +0,0 @@
-#ifndef MAP_H
-#define MAP_H
-
-#include "obsCarr.h"
-#include "pointParcours.h"
-#include "nVector.h"
-#include "controle.h"
-#include "Odometry.h"
-
-class map {
-public:
-    map (Odometry* nodo);
-    void addObs (obsCarr nobs);
-    void FindWay (point dep, point arr);
-    void FindWay (float depX, float depY, float arrX, float arrY);
-    void Execute (float XObjectif, float YObjectif);
-    nVector<pointParcours>& getParc () { return path; }
-    bool& getEnded () { return endedParc; }
-
-protected:
-    nVector<obsCarr> obs;
-    nVector<pointParcours> path;
-    bool endedParc; // Definit s'il existe un chemin parcourable dans le dernier FindWay
-    Odometry* Codo;
-};
-
-#endif
\ No newline at end of file

--- a/Map/nVector.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,168 +0,0 @@
-#ifndef VECTOR_H
-#define VECTOR_H
-
-template<class T>
-class elem {
-public:
-    elem () { ; }
-    elem (T nval) : val (nval) {  next = NULL; prev = NULL; }
-    elem (T nval, elem* nprev) : val (nval) { next = NULL; prev = nprev; }
-
-    elem<T> * gNext () { return next; }
-    elem<T> * gprev () { return prev; }
-    T& getVal () { return val; }
-
-    void sNext (elem<T> * nnext) { next = nnext; }
-    void sPrev (elem<T> * nprev) { prev = nprev; }
-
-private:
-    T val;
-    elem<T> * next;
-    elem<T> * prev;
-};
-
-template<class T>
-class nVector {
-public:
-    nVector () {
-        first = NULL;
-        curr = NULL;
-        init = false;
-        VectorSize = 0;
-    }
-
-    void push_back (T val) {
-        if (init) {
-            setCurrLast ();
-            elem<T> * n = new elem<T> (val, curr);
-            curr->sNext (n);
-            n->sPrev (curr);
-            VectorSize++;
-        }
-        else {
-            init = true;
-            first = new elem<T> (val);
-            VectorSize = 1;
-        }
-    }
-
-    void push_first (T val) {
-        if (init) {
-            curr = first;
-            elem<T> * n = new elem<T> (val);
-            curr->sPrev (n);
-            n->sNext (curr);
-            first = n;
-            VectorSize++;
-        }
-        else {
-            init = true;
-            first = new elem<T> (val);
-            VectorSize = 1;
-        }
-    }
-
-    bool erase (int index) {
-        bool worked = true;
-
-        if (init) {
-            if (index > 0 && index < VectorSize) {
-                if (setCurrIndex (index)) {
-                    elem<T>* p = curr->gprev ();
-                    if (p != NULL)
-                        p->sNext (curr->gNext ());
-
-                    elem<T>* n = curr->gNext ();
-                    if (n != NULL)
-                        n->sPrev (p);
-
-                    delete curr;
-
-                    VectorSize--;
-                    if (VectorSize <= 0) {
-                        init = false;
-                    }
-                }
-            }
-            else if (index == 0 && index < VectorSize) {
-                elem<T> * n = first->gNext ();
-
-                if (n != NULL) {
-                    n->sPrev (NULL);
-                    delete first;
-                    first = n;
-                    VectorSize--;
-                }
-                else {
-                    init = false;
-                    delete first;
-                    VectorSize = 0;
-                }
-            }
-            else {
-                worked = false;
-            }
-        }
-        else {
-            worked = false;
-        }
-        return worked;
-    }
-
-    void concatenate (nVector<T>& t) {
-        for (int i = 0; i < t.size (); ++i)
-            push_back (t[i]);
-    }
-
-    int size () { return VectorSize; }
-    bool empty () { return !init; }
-
-    T& operator[](int ind) {
-        setCurrIndex (ind);
-        return curr->getVal ();
-    }
-
-    void show () {
-        for (int i = 0; i < VectorSize; ++i) {
-            setCurrIndex (i);
-            std::cout << "(" << curr << ") Element " << i + 1 << " pere : " << curr->gprev() << ", fils " << curr->gNext () << std::endl;
-        }
-    }
-    
-    void clear () {
-        while (VectorSize != 0) {
-            erase (0);
-        }
-    }
-private:
-    void setCurrLast () {
-        if (init) {
-            curr = first;
-            while (curr->gNext () != NULL) {
-                curr = curr->gNext ();
-            }
-        }
-    }
-
-    bool setCurrIndex (int index) {
-        curr = first;
-        bool worked = true;
-
-        for (int i = 0; i < index; ++i) {
-            if (curr->gNext () != NULL)
-                curr = curr->gNext ();
-            else {
-                worked = false;
-                break;
-            }
-        }
-        return worked;
-    }
-
-    elem<T>* first;
-    elem<T>* curr;
-
-    bool init;
-    int VectorSize;
-};
-#endif
\ No newline at end of file

--- a/Map/obsCarr.cpp	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,31 +0,0 @@
-#include "obsCarr.h"
-
-bool obsCarr::getCroisement (point A, point B) {
-    point p0 (xcentre - dxtaille, ycentre + dytaille);
-    point p1 (xcentre + dxtaille, ycentre + dytaille);
-    point p2 (xcentre - dxtaille, ycentre - dytaille);
-    point p3 (xcentre + dxtaille, ycentre - dytaille);
-
-    if (CroisementSegment (p0, p3, A, B) || CroisementSegment (p1, p2, A, B) || belongs(A) || belongs(B))
-        return true;
-    else
-        return false;
-}
-
-bool obsCarr::getCroisement (float X, float Y, point B) {
-    point A (X, Y);
-    return getCroisement (A, B);
-}
-
-points4 obsCarr::getPoints () {
-    return {
-        point (xcentre - (dxtaille + MINDISTROBOT), ycentre + (dytaille + MINDISTROBOT)),
-        point (xcentre + (dxtaille + MINDISTROBOT), ycentre + (dytaille + MINDISTROBOT)),
-        point (xcentre - (dxtaille + MINDISTROBOT), ycentre - (dytaille + MINDISTROBOT)),
-        point (xcentre + (dxtaille + MINDISTROBOT), ycentre - (dytaille + MINDISTROBOT)),
-    };
-}
-
-bool obsCarr::belongs (point& A) {
-    return (A.getX () <= xcentre + dxtaille && A.getX () >= xcentre - dxtaille && A.getY () <= ycentre + dytaille && A.getY () >= ycentre - dytaille);
-}

--- a/Map/obsCarr.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,31 +0,0 @@
-#ifndef OBSCARR_H
-#define OBSCARR_H
-
-#include "controle.h"
-#include "figure.h"
-#include "point.h"
-
-class obsCarr : public figure {
-public:
-    obsCarr (float xc, float yc, float dxt, float dyt) : figure (xc,yc) {
-        dxtaille = dxt;
-        dytaille = dyt;
-    }
-
-    /* Retourne true si le segment AB croise le rectangle */
-    bool getCroisement (point A, point B);
-    /* Retourne true si le segment AB croise le rectangle */
-    bool getCroisement (float X, float Y, point B);
-
-    /* Retourne 4 Points pas tres loin du rectangle par lesquels peut passer le robot ! */
-    points4 getPoints ();
-    bool belongs (point& A);
-
-    float getDXT () { return dxtaille; }
-    float getDYT () { return dytaille; }
-
-protected:
-    float dxtaille, dytaille;
-};
-
-#endif
\ No newline at end of file

--- a/Map/point.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,44 +0,0 @@
-#ifndef POINT_H
-#define POINT_H
-
-typedef struct P4 points4;
-
-class point {
-public:
-    point (float nx, float ny) {
-        x = nx;
-        y = ny;
-    }
-    point () { ; }
-
-    float getX () { return x; }
-    float getY () { return y; }
-    void setX(float nx) { x = nx; }
-    void setY(float ny) { y = ny; }
-    
-    float operator*(point& a) {
-        return calculDistance2 (x, y, a.getX(), a.getY ());
-    }
-    bool operator==(point& a) {
-        return (x == a.getX () && y == a.getY ());
-    }
-    bool operator!=(point& a) {
-        return !(*this == a);
-    }
-
-protected:
-    float calculDistance2(float x1, float y1, float x2, float y2) {
-        return ((x1-x2)*(x1 - x2) + (y1 - y2)*(y1 - y2));
-    }
-
-    float x, y;
-};
-
-struct P4 {
-    point p0;
-    point p1;
-    point p2;
-    point p3;
-};
-
-#endif
\ No newline at end of file

--- a/Map/pointParcours.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,58 +0,0 @@
-#ifndef POINTPARCOURS_H
-#define POINTPARCOURS_H
-#include "math.h"
-
-#include "controle.h"
-#include "point.h"
-
-typedef struct PP4 PointsParc4;
-
-class pointParcours : public point {
-public:
-    pointParcours (float nx, float ny, pointParcours * npere, point arr) : point (nx, ny) {
-        if (npere != NULL)
-            G2 = sqrt(npere->getG2() + calculDistance2 (nx, ny, npere->getX(), npere->getY ()));
-        else
-            G2 = 0;
-        pere = npere;
-        H2 = sqrt (calculDistance2 (nx, ny, arr.getX (), arr.getY ()));
-    }
-
-    pointParcours (point p, pointParcours * npere, point arr) : point (p) {
-        if (pere != NULL)
-            G2 = npere->getG2 () + calculDistance2 (p.getX(), p.getY(), npere->getX (), npere->getY ());
-        else
-            G2 = 0;
-        pere = npere;
-
-        H2 = sqrt(calculDistance2 (p.getX (), p.getY (), arr.getX (), arr.getY ()));
-    }
-
-    pointParcours * getPere () { return pere; }
-
-    long double getG2 () { return G2; }
-    long double getH2 () { return H2; }
-    long double getP2 () { return G2 + H2; }
-
-    bool operator==(pointParcours& a) {
-        // Autre version : return (x == a.getX () && y == a.getY () && a.getP2 () == G2 + H2);
-        return (x == a.getX () && y == a.getY ());
-    }
-
-    bool operator==(point& a) {
-        return (x == a.getX () && y == a.getY ());
-    }
-
-protected:
-    pointParcours * pere;
-    long double G2, H2;
-};
-
-struct PP4 {
-    pointParcours p0;
-    pointParcours p1;
-    pointParcours p2;
-    pointParcours p3;
-};
-
-#endif
\ No newline at end of file

--- a/Odometry/Odometry.cpp	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,287 +0,0 @@
-#include "Odometry.h"
-
-// M1 = Moteur droit, M2 = Moteur gauche
-
-Odometry::Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc) : roboclaw(rc)
-{
-    m_v = v;
-    m_distPerTick_left = diameter_left*PI/quadrature;
-    m_distPerTick_right = diameter_right*PI/quadrature;
-
-    roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);
-    roboclaw.ResetEnc();
-    // Erreur autorisée sur le déplacement en angle
-    erreur_ang = 0.01;
-
-    m_pulses_right = 0;
-    m_pulses_left = 0;
-    pos_prog = 0;
-    
-    paused = false;
-    
-    timer.stop();
-    timer.reset();
-    
-    wait_ms(100);
-}
-
-void Odometry::setPos(double x, double y, double theta)
-{
-    this->x = x;
-    this->y = y;
-    this->theta = theta;
-}
-void Odometry::getEnc()
-{
-    logger.printf("EncM1 : %d\tEncM2 : %d\n\r", roboclaw.ReadEncM1(), roboclaw.ReadEncM2());
-}
-
-void Odometry::setX(double x)
-{
-    this->x = x;
-}
-
-void Odometry::setY(double y)
-{
-    this->y = y;
-}
-
-void Odometry::setTheta(double theta)
-{
-    this->theta = theta;
-}
-
-void Odometry::update_odo(void)
-{
-    int32_t roboclawENCM1 = roboclaw.ReadEncM1(); 
-    int32_t roboclawENCM2 = roboclaw.ReadEncM2();
-    int32_t delta_right = roboclawENCM1 - m_pulses_right;
-    m_pulses_right = roboclawENCM1;
-    int32_t delta_left = roboclawENCM2 - m_pulses_left;
-    m_pulses_left = roboclawENCM2;
-    
-    double deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right)*C / 2.0f;
-    double deltaTheta = (m_distPerTick_left*delta_left - m_distPerTick_right*delta_right)*C / m_v;
-
-    /*double R = deltaS/deltaTheta;
-
-    double xO = x - R*sin(theta);
-    double yO = y + R*cos(theta);
-
-    theta += deltaTheta;
-
-    if(deltaTheta == 0) {
-        x = x + deltaS*cos(theta);
-        y = y + deltaS*sin(theta);
-    }
-    else {
-        x = xO + R*sin(theta);
-        y = yO - R*cos(theta);
-    }*/
-    
-    
-    
-    double dx = deltaS*cos(theta+deltaTheta/2);
-    double dy = deltaS*sin(theta+deltaTheta/2);
-    x += dx;
-    y += dy;
-    
-    theta += deltaTheta;
-    while(theta > PI) theta -= 2*PI;
-    while(theta <= -PI) theta += 2*PI;
-}
-
-void Odometry::GotoXY(double x_goal, double y_goal)
-{
-    saved_x_goal = x_goal;
-    saved_y_goal = y_goal;
-    
-    currentStep = STEP_D;
-    currentMainFunction = MAIN_FCT_XY;
-    
-    double theta_ = atan2(y_goal-y, x_goal-x);
-    double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    GotoThet(theta_);
-    GotoDist(dist_);
-}
-
-void Odometry::GotoXYT(double x_goal, double y_goal, double theta_goal)
-{
-    saved_x_goal = x_goal;
-    saved_y_goal = y_goal;
-    saved_theta_goal = theta_goal;
-    
-    currentStep = STEP_D;
-    currentMainFunction = MAIN_FCT_XYT;
-    
-    double theta_ = atan2(y_goal-y, x_goal-x);
-    double dist_ = sqrt(carre(x_goal-x)+carre(y_goal-y));
-    GotoThet(theta_);
-    GotoDist(dist_);
-    GotoThet(theta_goal);
-}
-
-void Odometry::GotoThet(double theta_)
-{
-    saved_theta_goal = theta_;
-    currentStep = STEP_T;
-    //pos_prog++;
-    //logger.printf("Theta : %3.2f\n\r", theta_*180/PI);
-    //arrived = false;
-    int32_t distance_ticks_left;
-    int32_t distance_ticks_right;
-
-    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
-
-    // Le calcul d'erreur est bon (testé), tu peux le vérifier par dessin
-    double erreur_theta = theta_ - getTheta();
-
-    while(erreur_theta >= PI) erreur_theta -= 2*PI;
-    while(erreur_theta < -PI) erreur_theta += 2*PI;
-    
-    if(erreur_theta < 0) {
-        distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
-        distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
-    } else {
-        distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
-        distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
-    }
-
-    //logger.printf("TV %3.2f\tTh %3.2f\tET %3.2f\n\r",theta_*180/PI,getTheta()*180/PI,erreur_theta*180/PI);
-    //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
-    //logger.printf("M1 %6d\tM2 %6d\n\r",distance_ticks_right, distance_ticks_left);
-
-    roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
-
-    //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
-
-    //while(abs(m_pulses_right - distance_ticks_right) >= 1 && abs(m_pulses_left - distance_ticks_left) >= 1) wait(0.001);
-    
-    while(abs(erreur_theta) > 2.f*PI/180.f)
-    {
-        wait(0.001);
-        
-        erreur_theta = theta_ - getTheta();
-        while(erreur_theta >= PI) erreur_theta -= 2*PI;
-        while(erreur_theta < -PI) erreur_theta += 2*PI;
-    }
-    
-    //logger.printf ("[Thet] %d\t%d\n\r", m_pulses_right - distance_ticks_right, m_pulses_left - distance_ticks_left); 
-    //logger.printf("%6d\t%6d\t%6d\t%6d\t%6d\n\r",m_pulses_right - pos_initiale_right, distance_ticks_right, m_pulses_left - pos_initiale_left, distance_ticks_left);
-    
-    wait(0.4);
-    
-    /*roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);*/
-    
-    //theta = theta_;
-    //arrived = true;
-    //logger.printf("arrivey %d\n\r",pos_prog);
-}
-
-void Odometry::GotoDist(double distance)
-{
-    currentStep = STEP_D;
-    //pos_prog++;
-    //logger.printf("Dist : %3.2f\n\r", distance);
-    //arrived = false;
-
-    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
-
-    int32_t distance_ticks_right = (int32_t) distance/m_distPerTick_right + pos_initiale_right;
-    int32_t distance_ticks_left = (int32_t) distance/m_distPerTick_left + pos_initiale_left;
-    if (distance >= 0)
-        roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
-    else
-        roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, -vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, -vitesse_dista, deccel_dista, distance_ticks_left, 1);   
-    //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
-    
-    while(abs(m_pulses_right - distance_ticks_right) >= 30 || abs(m_pulses_left - distance_ticks_left) >= 30) //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
-    {  
-        wait(0.1);
-    }
-    
-    /*while( carre(x-saved_x_goal) + carre(y-saved_y_goal) > 4*4)
-    {
-        wait(0.1);
-        logger.printf("Dist error : %f  (%f,%f -> %f,%f)\r\n", sqrt(carre(x-saved_x_goal) + carre(y-saved_y_goal)),x,y,saved_x_goal,saved_y_goal);
-    }*/
-    
-    wait(0.4);
-    
-    /*roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);*/
-    
-    //logger.printf("arrivey %d\n\r",pos_prog);
-    //logger.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", getX(), getY(), getTheta()*180/PI);
-}
-
-void Odometry::TestEntraxe(int i) {
-    int32_t distance_ticks_left;
-    int32_t distance_ticks_right;
-
-    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
-
-    double erreur_theta = 2*PI*i - getTheta();
-    if(erreur_theta < 0) {
-        distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
-        distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
-    } else {
-        distance_ticks_left = (int32_t) pos_initiale_left + (erreur_theta*m_v/2)/m_distPerTick_left;
-        distance_ticks_right = (int32_t) pos_initiale_right - (erreur_theta*m_v/2)/m_distPerTick_right;
-    }
-    
-    roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
-    
-    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left));
-        
-    wait(0.4);
-}
-
-void Odometry::Forward(float i) {    
-    int32_t pos_initiale_right = m_pulses_right, pos_initiale_left = m_pulses_left;
-
-    int32_t distance_ticks_right = (int32_t) i/m_distPerTick_right + pos_initiale_right;
-    int32_t distance_ticks_left = (int32_t) i/m_distPerTick_left + pos_initiale_left;
-
-    roboclaw.SpeedAccelDeccelPositionM1M2(accel_dista, vitesse_dista, deccel_dista, distance_ticks_right, accel_dista, vitesse_dista, deccel_dista, distance_ticks_left, 1);
-
-    //logger.printf("IniR:%6d\tDistR:%6d\tIniL:%6d\tDistL:%6d\n\r", pos_initiale_right, distance_ticks_right, pos_initiale_left, distance_ticks_left);
-    
-    while((m_pulses_right != distance_ticks_right)&&(m_pulses_left != distance_ticks_left)); //logger.printf("PR:%6d\tIR:%6d\tDR:%6d\tPL:%6d\tIL:%6d\tDL:%6d\n\r",m_pulses_right, pos_initiale_right, distance_ticks_right, m_pulses_left, pos_initiale_left, distance_ticks_left);
-    wait(0.4);
-}
-
-void Odometry::stop() { 
-    paused = false; 
-    timer.stop();
-    timer.reset();
-    
-    roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);
-}
-
-void Odometry::pause() { 
-    timer.start();
-    timer.reset();
-    paused = true;
-    roboclaw.ForwardM1(0);
-    roboclaw.ForwardM2(0);
-}
-
-void Odometry::resume() {    
-    if(paused && timer.read() > 0.5f)
-    {
-        paused = false;
-        timer.stop();
-        timer.reset();
-        
-        if(currentMainFunction == MAIN_FCT_XY)
-            GotoXY(saved_x_goal,saved_y_goal);
-        else
-            GotoXYT(saved_x_goal,saved_y_goal,saved_theta_goal);
-            
-        
-    }
-}
\ No newline at end of file

--- a/Odometry/Odometry.h	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,156 +0,0 @@
-#ifndef ODOMETRY_H
-#define ODOMETRY_H
-
-#include "DefinesSharps.h"
-#include "mbed.h"
-#include "../RoboClaw/RoboClaw.h"
-
-#define PI 3.1415926535897932384626433832795
-#define C 1.0
-
-/* Vitesse d'acceleration d'angle reduite de 8000->4000 */
-#define accel_angle 12000
-#define vitesse_angle 10000
-#define deccel_angle 12000
-
-#define accel_dista 12000
-#define vitesse_dista 12000
-#define deccel_dista 12000
-
-/* Si le robot parcourt un trop grand angle, il diminuer l'entraxe, sinon l'augmenter */
-#define ENTRAXE 243.8
-
-#define STEP_T 0
-#define STEP_D 1
-
-#define MAIN_FCT_XY 0
-#define MAIN_FCT_XYT 1
-
-/*
-*   Author : Benjamin Bertelone, reworked by Simon Emarre
-*/
-
-extern Serial logger;
-
-/** Permet la gestion de l'odometrie du robot **/
-class Odometry
-{
-public:
-    /** Constructeur standard
-    * @param diameter_right Definit le diametre de la roue droite
-    * @param diameter_left Definit le diametre de la roue gauche
-    * @param v Definit l'entraxe du robot
-    * @param rc Definit une reference vers l'objet permettant l'asserv des moteurs
-    * @note Cet objet doit etre initialise en amont
-    */
-    Odometry(double diameter_right, double diameter_left, double v, uint16_t quadrature, RoboClaw &rc);
-
-    /** Demande au robot d'effectuer un certain nombre de tour sur lui même */
-    void TestEntraxe(int i);
-
-    /** Demande au robot d'effectuer un déplacement sur l'avant. Voir si l'on peut enlever la correction PID */
-    void Forward(float i);
-
-    /** Les fonctions suivantes permettent de reinitialiser les valeurs de position de l'odometrie.
-    */
-    void setPos(double x, double y, double theta);
-    void setX(double x);
-    void setY(double y);
-    void setTheta(double theta);
-
-    /** Les fonctions suivantes permettent de deplacer le robot vers une position ou un angle voulu
-    */
-    void GotoXY(double x_goal, double y_goal);
-    void GotoXYT(double x_goal, double y_goal, double theta_goal);
-    void GotoThet(double theta_);
-    void GotoDist(double distance);
-
-    double getX() {
-        return x;
-    }
-    double getY() {
-        return y;
-    }
-    double getTheta() {
-        return theta;   // ]-PI;PI]
-    }
-    double getTheta_(double x, double y);
-
-    double abs_d(double x) {
-        if(x<0) return -x;
-        else return x;
-    }
-
-    /* Les fonction suivantes sont actuellement inutilisables, elles pourraient buguer en cas d'utilisation. */
-
-    double getVitLeft() {
-        return m_vitLeft;
-    }
-    double getVitRight() {
-        return m_vitRight;
-    }
-
-    double getDistLeft() {
-        return m_distLeft;
-    }
-    double getDistRight() {
-        return m_distRight;
-    }
-
-    void setDistLeft(double dist) {
-        m_distLeft = dist;
-    }
-    void setDistRight(double dist) {
-        m_distRight = dist;
-    }
-
-    double calcul_distance(double x, double y, double theta_goal);
-
-    int32_t getPulsesLeft(void) {
-        return m_pulses_left;
-    }
-    int32_t getPulsesRight(void) {
-        return m_pulses_right;
-    }
-    double carre(double a) {
-        return a*a;
-    }
-    
-    void getEnc();
-    
-    /** La fonction retourne vraie quand le robot atteint l'angle voulu avec une marge d'erreur definie par la fonction
-    * @param theta_ valeur de l'angle devant etre atteint
-    */
-    bool isArrived(void) {return arrived;}
-    /** Permet de mettre à jour les valeurs de l'odometrie
-    */
-    void update_odo(void);
-    
-    void stop();
-    void pause();
-    void resume();
-    char getCurrentStep(){return currentStep;}
-
-private:
-    RoboClaw &roboclaw;
-    int32_t m_pulses_left;
-    int32_t m_pulses_right;
-    uint8_t pos_prog;
-    double x, y, theta;
-    double m_vitLeft, m_vitRight;
-    double m_distLeft, m_distRight;
-    
-    double saved_x_goal, saved_y_goal, saved_theta_goal;
-    char currentStep;
-    char currentMainFunction;
-    bool paused;
-    
-    double m_distPerTick_left, m_distPerTick_right, m_v;
-
-    double erreur_ang;
-    bool arrived;
-    
-    Timer timer;
-};
-
-#endif

--- a/StepperMotor.lib	Thu May 05 15:11:06 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/sype/code/StepperMotor/#c7011e72f0c7

--- a/main.cpp	Thu May 05 15:11:06 2016 +0000
+++ b/main.cpp	Thu May 05 16:01:55 2016 +0000
@@ -10,13 +10,99 @@
 #define vitesse_dista 12000
 #define deccel_dista 12000
 
+void Sharps();
+BusOut drapeau (PC_8, PC_6, PC_5);
+
 RoboClaw roboclaw(ADR, 460800, PA_11, PA_12);
+AnalogIn Rcapt1(PA_4);
+int RvalRcapt1 = 0;
+AnalogIn Rcapt2(PB_0);
+int RvalRcapt2 = 0;
+AnalogIn Rcapt3(PC_1);
+int RvalRcapt3 = 0;
 
+int Ravance = 1;
+#define R_SEUIL_SHARP 0.3
+
+void GotoDist(float timer) {
+    Timer t;
+    t.reset();
+    roboclaw.SpeedAccelM1M2(accel_dista, vitesse_dista, vitesse_dista);
+    t.start();
+    
+    while (t.read() < timer) {
+        if (Ravance != 1) {
+            roboclaw.ForwardM1(0);
+            roboclaw.ForwardM2(0);
+            t.stop();
+            while (Ravance != 1);
+            t.start();
+        }
+    }
+    
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    t.stop();
+    t.reset();
+}
+
+void GotoThet (float timer) {
+    Timer t;
+    t.reset();
+    t.start();
+    roboclaw.SpeedAccelM1M2(accel_dista, vitesse_dista, -vitesse_dista);
+    
+    while (t.read() < timer) {
+        if (Ravance != 1) {
+            roboclaw.ForwardM1(0);
+            roboclaw.ForwardM2(0);
+            t.stop();
+            while (Ravance != 1);
+            t.start();
+        }
+    }
+    
+    roboclaw.ForwardM1(0);
+    roboclaw.ForwardM2(0);
+    
+    t.stop();
+    t.reset();
+}
 /* Debut du programme */
 int main(void)
 {
-    int distance_ticks_right = 10000;
-    int distance_ticks_left = 10000;
-    roboclaw.SpeedAccelDeccelPositionM1M2(accel_angle, vitesse_angle, deccel_angle, distance_ticks_right, accel_angle, vitesse_angle, deccel_angle, distance_ticks_left, 1);
+    Ticker ticker;
+    ticker.attach(&Sharps, 0.01);
+    
+    GotoDist(7);
+    GotoThet(7);
+    
     while(1);
 }
+
+void Sharps() {
+    if (Rcapt1.read() > R_SEUIL_SHARP) RvalRcapt1++;
+    else RvalRcapt1--;
+    RvalRcapt1 = RvalRcapt1 > 10 ? 10 : RvalRcapt1;
+    RvalRcapt1 = RvalRcapt1 < 0 ? 0 : RvalRcapt1;
+    
+    if (Rcapt2.read() > R_SEUIL_SHARP) RvalRcapt2++;
+    else RvalRcapt2--;
+    RvalRcapt2 = RvalRcapt2 > 10 ? 10 : RvalRcapt2;
+    RvalRcapt2 = RvalRcapt2 < 0 ? 0 : RvalRcapt2;
+    
+    if (Rcapt3.read() > R_SEUIL_SHARP) RvalRcapt3++;
+    else RvalRcapt3--;
+    RvalRcapt3 = RvalRcapt3 > 10 ? 10 : RvalRcapt3;
+    RvalRcapt3 = RvalRcapt3 < 0 ? 0 : RvalRcapt3;
+    
+    if (RvalRcapt1 >= 5 || RvalRcapt2 >= 5 || RvalRcapt3 >= 5)
+        Ravance = 0;
+    else 
+        Ravance = 1;
+        
+    if (Ravance == 0)
+        drapeau = 1;
+    else 
+        drapeau = 2;
+}
\ No newline at end of file