ARES
/
Robot 2016
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: mbed
Revision 0:b127c787a51b, committed 2015-05-24
- Comitter:
- Jagang
- Date:
- Sun May 24 12:30:47 2015 +0000
- Commit message:
- Nettoyage du code d'asserv.; L'asserv ne fonctionne plus, juste test, moteurs ? 20% sur 1m
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AX12/AX12.cpp Sun May 24 12:30:47 2015 +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 Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,205 @@
+/* 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 abs(x) (x<0?-x:x)
+
+//#define AX12_WRITE_DEBUG 0
+//#define AX12_READ_DEBUG 0
+//#define AX12_TRIGGER_DEBUG 0
+//#define AX12_DEBUG
+
+#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/Asservissement/Asservissement.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,8 @@
+#include "Asservissement.h"
+
+
+Asservissement::Asservissement(Odometry &odometry, Motor &motorL, Motor &motorR)
+:m_odometry(odometry),m_motorL(motorL),m_motorR(motorR)
+{
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement/Asservissement.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,30 @@
+#ifndef ASSERVISSEMENT_H
+#define ASSERVISSEMENT_H
+
+#include "mbed.h"
+#include "Odometry.h"
+#include "Motor.h"
+
+class Asservissement
+{
+public:
+ Asservissement(Odometry &odometry, Motor &motorL, Motor &motorR);
+ virtual void update(float dt) = 0;
+
+ virtual void setGoal(float x, float y, float theta) = 0;
+ virtual void setGoal(float x, float y) = 0;
+
+ virtual void stop() = 0;
+
+ virtual bool isArrived() = 0;
+
+ virtual char getState() = 0;
+
+protected:
+ Odometry &m_odometry;
+ Motor &m_motorL;
+ Motor &m_motorR;
+};
+
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement/Asservissement_v1.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,46 @@
+#include "Asservissement_v1.h"
+
+Asservissement_v1::Asservissement_v1(Odometry &odometry, Motor &motorL, Motor &motorR)
+:Asservissement(odometry,motorL,motorR)
+{
+
+}
+
+void Asservissement_v1::update(float dt)
+{
+ if(m_odometry.getY()<1000 && m_odometry.getX()<1000)
+ {
+ m_motorL.setSpeed(0.2);
+ m_motorR.setSpeed(0.2);
+ }
+ else
+ {
+ m_motorL.setSpeed(0);
+ m_motorR.setSpeed(0);
+ }
+}
+
+void Asservissement_v1::setGoal(float x, float y, float theta)
+{
+
+}
+
+void Asservissement_v1::setGoal(float x, float y)
+{
+
+}
+
+void Asservissement_v1::stop()
+{
+
+}
+
+bool Asservissement_v1::isArrived()
+{
+ return false;
+}
+
+char Asservissement_v1::getState()
+{
+ return m_state;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement/Asservissement_v1.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,25 @@
+#ifndef ASSERVISSEMENT_V1_H
+#define ASSERVISSEMENT_V1_H
+
+#include "Asservissement.h"
+
+class Asservissement_v1: public Asservissement
+{
+public:
+ Asservissement_v1(Odometry &odometry, Motor &motorL, Motor &motorR);
+ virtual void update(float dt);
+
+ virtual void setGoal(float x, float y, float theta);
+ virtual void setGoal(float x, float y);
+
+ virtual void stop();
+
+ virtual bool isArrived();
+
+ virtual char getState();
+private:
+ char m_state;
+};
+
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement/Motor.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,84 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+
+#include "mbed.h"
+
+#define abs(x) (x<0?-x:x)
+
+/** Motor
+ * Classe permettant de controler un moteur
+ */
+class Motor
+{
+ private:
+ PwmOut pwm;
+ DigitalOut dir;
+ bool inverse;
+ float speed;
+
+ public:
+ /** Création d'un moteur
+ *
+ * @param pwm Pin sur laquelle sort la PWM
+ * @param dir Pin sur laquelle sort le bit de direction
+ */
+ Motor(PinName pin_pwm, PinName pin_dir): pwm(pin_pwm), dir(pin_dir)
+ {
+ inverse = false;
+ speed = 0.0f;
+ pwm.period_us(10);
+ pwm = 0.0f;
+
+ }
+
+ virtual ~Motor(){}
+
+ /** Fixe la vitesse du moteur
+ *
+ * @param s Vitesse du moteur entre -1 et 1
+ */
+ void setSpeed(float s)
+ {
+ float abss = abs(s);
+ speed = s;
+ pwm = abss;
+ if(s > 0)
+ dir = !inverse;
+ else
+ dir = inverse;
+ }
+
+ /** Retourne la vitesse théorique du moteur
+ *
+ * @return Vitesse du moteur entre -1 et 1
+ */
+ float getSpeed(){return speed;}
+
+ /** Fixe l'invesrion de polarité
+ *
+ * Exemple :
+ * @code
+ * moteur.setSpeed(1); // le moteur tourne à pleine vitesse dans le sens trigo
+ * moteur.setInverse(true); // le moteur tourne à pleine vitesse dans le sens anti-trigo
+ * moteur.setSpeed(-1); // le moteur tourne à pleine vitesse dans le sens trigo
+ * @endcode
+ *
+ * @param i Inversion de polarité
+ */
+ void setInverse(bool i)
+ {
+ inverse = i;
+
+ setSpeed(speed);
+ }
+
+ /** Retourne l'état d'inversion de polarité
+ *
+ * @return True ou False
+ */
+ bool getInverse(){return inverse;}
+
+};
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Asservissement/PIDcontroller.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,95 @@
+#ifndef PIDcontroller_H
+#define PIDcontroller_H
+
+class PIDcontroller
+{
+ private :
+ float Kp;
+ float Ki;
+ float Kd;
+
+ float err;
+ float err_old;
+ float err_sum;
+ float consigne;
+ float value;
+
+
+ public :
+
+ PIDcontroller()
+ {
+ Kp = (float)1;
+ Ki = (float)1;
+ Kd = (float)1;
+ err = (float)0;
+ err_old = (float)0;
+ err_sum = (float)0;
+ consigne = (float)0;
+ value =(float)0;
+ }
+
+ PIDcontroller(float Kp, float Ki, float Kd)
+ {
+ this->Kp = Kp;
+ this->Ki = Ki;
+ this->Kd = Kd;
+ err = (float)0;
+ err_old = (float)0;
+ err_sum = (float)0;
+ consigne = (float)0;
+ value = (float)0;
+ }
+
+ ~PIDcontroller()
+ {
+
+ }
+
+ void reset()
+ {
+ err_old = (float)0;
+ err_sum = (float)0;
+ }
+ void setConsigne(float consigne)
+ {
+ this->consigne = consigne;
+ }
+
+ float update(float currentValue, float dt = (float)0.1)
+ {
+ err = consigne - currentValue;
+ err_sum = err_sum + err;
+ float err_diff = err-err_old;
+ err_old = err;
+
+ value = Kp*(err + Ki*err_sum + Kd*err_diff/dt);
+
+ return value;
+ }
+
+ void setKp(float Kp)
+ {
+ this->Kp = Kp;
+ }
+
+ void setKi(float Ki)
+ {
+ this->Ki = Ki;
+ }
+
+ void setKd(float Kd)
+ {
+ this->Kd = Kd;
+ }
+
+ void set(float Kp, float Ki,float Kd)
+ {
+ setKp(Kp);
+ setKi(Ki);
+ setKd(Kd);
+ }
+
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Map.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,410 @@
+#include "Map.h"
+
+#include "Obs_circle.h"
+#include "Obs_rect.h"
+
+#ifdef CODEBLOCK
+ using namespace std;
+ #include <iostream>
+ #include <fstream>
+#else
+ #include "mbed.h"
+ extern Serial logger;
+#endif
+#include <math.h>
+
+Map::Map()
+{
+
+}
+
+Map::~Map()
+{
+ for(unsigned int i=0;i<obstacles.size();i++)
+ delete obstacles[i];
+}
+
+void Map::build()
+{
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MG,-100,-100,2100,0));// MG
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MB,2100,-100,2000,3100));// MB
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MH,-100,-100,0,3100));// MH
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_MD,2100,3000,-100,3100));// MD
+
+
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M1,778,0,800,450));// M1
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M2,1200,0,1222,450));// M2
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M3,800,0,1200,70));// M3
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M4,1222,2550,1200,3000));// M4
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M5,1200,2930,800,3000));// M5
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_M6,800,2550,778,3000));// M6
+
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_E,580,967,0,2033));// E
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_S,2000,1200,1900,1800));// S
+
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D1,70,265,0,335));// D1
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D2,70,565,0,636));// D2
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D3,70,2365,0,2435));// D3
+ obstacles.push_back(new Obs_rect(ROBOTRADIUS,IDO_D4,70,2665,0,2735));// D4
+
+
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC1,800,910,35));// PC1
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC3,1650,1500,35));// PC3
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC2,1750,250,35));// PC2
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC4,1750,3000-250,35));// PC4
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_PC5,800,3000-910,35));// PC5
+
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P1,200,90,30));// P1
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P2,100,850,30));// P2
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P3,200,850,30));// P3
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P4,1355,870,35));// P4
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P5,1750,90,30));// P5
+ //obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P6,1850,90,30));// P6
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P7,1770,1100,30));// P7
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P8,1400,1300,30));// P8
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P9,200,3000-90,30));// P9
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P10,100,3000-850,30));// P10
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P11,200,3000-850,30));// P11
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P12,1355,3000-870,30));// P12
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P13,1750,3000-90,30));// P13
+ //obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P14,1850,3000-90,30));// P14
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P15,1770,3000-1100,30));// P15
+ obstacles.push_back(new Obs_circle(ROBOTRADIUS,IDO_P16,1400,3000-1300,30));// P16
+}
+
+int Map::getHeight(float x, float y)
+{
+ int height = 0;
+ for(unsigned int i=0;i<obstacles.size();i++)
+ height += obstacles[i]->height(x,y);
+ return height;
+}
+
+float dist(Point *p1,Point *p2)
+{
+ int dx = p1->getx()-p2->getx();
+ if(dx<0) dx=-dx;
+ int dy = p1->gety()-p2->gety();
+ if(dy<0) dy=-dy;
+ return sqrt((float)dx*dx+dy*dy);
+}
+
+char Map::AStar(float x, float y, float goal_x, float goal_y, float mpc)
+{
+ /*! http://www.gamedev.net/page/resources/_/technical/artificial-intelligence/a-pathfinding-for-beginners-r2003 !*/
+ //float dx,dy; // Permet de diminuer l'erreur par rapport au centre des cases
+ //dx = ((((int)(x/mpc))*mpc-mpc/2)+(((int)(goal_x/mpc))*mpc-mpc/2))/2;
+
+ path.clear();
+
+ Point goal(goal_x/mpc,goal_y/mpc);
+ if(getHeight(goal_x,goal_y) >= 32000)
+ {
+ #if LOG_LEVEL >= 2
+ logger.printf("[warning - pathfinder] Unreachable point (%.3f,%.3f)\r\n",goal_x,goal_y);
+ #endif
+ return 4;
+ }
+
+ if(getHeight(x,y) >= 32000)
+ {
+ #if LOG_LEVEL >= 2
+ logger.printf("[warning - pathfinder] Unstartable point (%.3f,%.3f)\r\n",x,y);
+ #endif
+ return 5;
+ }
+
+
+ unsigned int i=0;
+ unsigned int instanceDePoint=0;
+
+ std::vector<Point*> openList;
+ openList.push_back(new Point(x/mpc,y/mpc));
+ openList[0]->setG(0);
+ openList[0]->setH(dist(openList[0],&goal));
+ openList[0]->setParent();
+
+ std::vector<Point*> closeList;
+
+ Point* current;
+ do
+ {
+
+ // On cherche le plus petit F dans la liste ouverte
+ current = openList[0];
+
+ unsigned int pos = 0;
+ for(i=0;i<openList.size();i++)
+ if(openList[i]->getF() < current->getF())
+ {
+ current = openList[i];
+ pos = i;
+ }
+
+ // On le place dans la liste fermé
+ closeList.push_back(current);
+ openList.erase(openList.begin()+pos);
+
+ #if LOG_LEVEL >= 4 && LOG_ASTAR
+ logger.printf("[info - pathfinder] Adding (%d,%d) in the closed list\r\n",current->getx(),current->gety());
+ logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
+ #endif
+
+ // On ajoute tous ses voisins viable das la liste ouverte
+ for(int dx=-1;dx<=1;dx++)
+ {
+ for(int dy=-1;dy<=1;dy++)
+ {
+ if(dx==0 && dy==0)
+ continue;
+ if(!(dx == 0 || dy == 0)) // On skype les mouvement diagoneaux
+ continue;
+
+ Point *p = new Point(current->getx()+dx,current->gety()+dy);
+ instanceDePoint++;
+ if(p == 0 || instanceDePoint >= MAXPOINT) // Overload !!!
+ {
+ for(unsigned int i=0;i<openList.size();i++)
+ delete openList[i];
+ for(unsigned int i=0;i<closeList.size();i++)
+ delete closeList[i];
+
+ path.clear();
+
+ #if LOG_LEVEL >= 1
+ logger.printf("[error - pathfinder] Overload (%d,%d)\r\n",openList.size(),closeList.size());
+ #endif
+
+ return 3;
+ }
+
+ if(p->in(closeList)) // On ignore le point si il est déjà dans la liste fermé
+ {
+ delete p;
+ continue;
+ }
+
+ int height = getHeight((current->getx()+dx)*mpc,(current->gety()+dy)*mpc);
+ if(height>=32000) // On ignore le point, il n'est pas accessible
+ {
+ delete p;
+ continue; // On ignore le point si il est déjà dans la liste fermé
+ }
+
+ if(p->in(openList,pos))
+ {
+ if(dx == 0 || dy == 0) // Mouvement non diagonnal
+ {
+ if(current->getG() + NDIAG_COST < openList[pos]->getG())
+ {
+ openList[pos]->setG(current->getG() + NDIAG_COST);
+ openList[pos]->setParent(current);
+ }
+ }
+ else
+ {
+ if(current->getG() + DIAG_COST < openList[pos]->getG())
+ {
+ openList[pos]->setG(current->getG() + DIAG_COST);
+ openList[pos]->setParent(current);
+ }
+ }
+
+ delete p;
+ }
+ else
+ {
+ openList.push_back(p);
+ if(dx == 0 || dy == 0) // Mouvement non diagonnal
+ p->setG(current->getG() + NDIAG_COST);
+ else
+ p->setG(current->getG() + DIAG_COST);
+ p->setH(height + dist(p,&goal));
+ p->setParent(current);
+ }
+ }
+ }
+ }
+ while(dist(closeList.back(),&goal) && !openList.empty()); // Tant qu'on a pas atteint la case et qu'on a des choix
+
+
+ #if LOG_LEVEL >= 3
+ logger.printf("[info - pathfinder] Closed list : %d elements\r\n[info - pathfinder] Opened list : %d elements\r\n",openList.size(),closeList.size());
+ #endif
+
+ if(!openList.empty())
+ {
+ #ifdef CODEBLOCK
+ ofstream f_openlist("openlist.txt");
+ for(i=0;i<openList.size();i++)
+ {
+ f_openlist << i << "," << openList[i]->getx()*mpc << "," << openList[i]->gety()*mpc << endl;
+ delete openList[i];
+ }
+ f_openlist.close();
+
+ ofstream f_closelist("closelist.txt");
+ for(i=0;i<closeList.size();i++)
+ {
+ f_closelist << i << "," << closeList[i]->getx()*mpc << "," << closeList[i]->gety()*mpc << endl;
+ }
+ f_closelist.close();
+ #endif
+
+ // On reconstruit le chemin
+ #if LOG_LEVEL >= 3
+ logger.printf("[info - pathfinder] Recontruction du chemin ... ");
+ #endif
+ path.clear();
+ Point* c = closeList.back();
+ while(c != 0)
+ {
+ path.insert(path.begin(),SimplePoint(c->getx()*mpc,c->gety()*mpc));
+ c = c->getParent();
+ }
+
+ path.front().x=x;
+ path.front().y=y;
+
+ path.back().x=goal_x;
+ path.back().y=goal_y;
+
+ #ifdef CODEBLOCK
+ ofstream f_path("path.txt");
+ for(i=0;i<path.size();i++)
+ {
+ f_path << i << "," << path[i].x << "," << path[i].y << endl;
+ }
+ f_path.close();
+ #endif
+
+ for(i=0;i<closeList.size();i++)
+ delete closeList[i];
+
+ #if LOG_LEVEL >= 3
+ logger.printf("[done] %d elements\r\n",path.size());
+ logger.printf("[info - pathfinder] Tendage du chemin ... ");
+ #endif
+
+
+ // Algo de 'tendage' du chemin
+ bool continuer = true;
+ unsigned int pointValide = 0;
+
+ #ifdef CODEBLOCK
+ ofstream f_tendage("tendage.txtt");
+ #endif // CODEBLOCK
+
+
+ while(continuer)
+ {
+ continuer = false;
+
+ for(unsigned int i1=pointValide;i1<path.size();i1++)
+ {
+ bool necessaire = false;
+
+ for(unsigned int i2=i1+2;i2<path.size();i2++)
+ {
+ //cout << "Entre " << i1 << " et " << i2 << endl;
+ if(path[i1].x!=path[i2].x && atan((path[i1].y-path[i2].y)/(path[i1].x-path[i2].x)) < PI/4)
+ {
+ float a = (path[i1].y-path[i2].y)/(path[i1].x-path[i2].x);
+ float b = path[i1].y - a*path[i1].x;
+ float step = (mpc*0.5f)*cos(atan(a));
+
+ necessaire = false;
+
+ for(x=min(path[i1].x,path[i2].x);x<max(path[i1].x,path[i2].x);x+=step)
+ {
+ y=a*x+b;
+
+ #ifdef CODEBLOCK
+ f_tendage << getHeight(x,y) << "," << x << "," << y << endl;
+ #endif // CODEBLOCK
+
+ if(getHeight(x,y) >= 32000)
+ {
+ necessaire = true;
+ break;
+ }
+ }
+
+ if(!necessaire)
+ {
+ // Ca n'a pas touché, on peut supprimmer le point entre les deux
+ continuer = true;
+ path.erase(path.begin()+i2-1);
+ }
+ else
+ pointValide++;
+ break;
+ }
+ else
+ {
+ // x=a*y+b
+ float a = (path[i1].x-path[i2].x)/(path[i1].y-path[i2].y);
+ float b = path[i1].x - a*path[i1].y;
+ float step = (mpc*0.5f)*cos(atan(a));
+
+ necessaire = false;
+
+ for(y=min(path[i1].y,path[i2].y);y<max(path[i1].y,path[i2].y);y+=step)
+ {
+ x=a*y+b;
+
+ #ifdef CODEBLOCK
+ f_tendage << getHeight(x,y) << "," << x << "," << y << endl;
+ #endif // CODEBLOCK
+
+ if(getHeight(x,y) >= 32000)
+ {
+ necessaire = true;
+ break;
+ }
+ }
+
+ if(!necessaire)
+ {
+ // Ca n'a pas touché, on peut supprimmer le point entre les deux
+ continuer = true;
+ path.erase(path.begin()+i2-1);
+ }
+ else
+ pointValide++;
+ break;
+
+ }
+ }
+ if(continuer)
+ break;
+ }
+ }
+
+ #ifdef CODEBLOCK
+ f_tendage.close();
+ #endif
+
+
+
+ #if LOG_LEVEL >= 3
+ logger.printf("[done] %d elements\r\n",path.size());
+ #endif
+
+ return 1;
+ }
+ else
+ {
+ for(i=0;i<openList.size();i++)
+ delete openList[i];
+ for(i=0;i<closeList.size();i++)
+ delete closeList[i];
+
+ path.clear();
+
+ return 2;
+ }
+}
+
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Map.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,49 @@
+#ifndef MAP_H
+#define MAP_H
+
+#include "includes.h"
+
+#include "Obstacle.h"
+#include "Point.h"
+#include <vector>
+
+#define DIAG_COST 0.7071067/2
+#define NDIAG_COST 0.5/2
+
+#define LOG_LEVEL 2 //4 debug(very slow) - 3 errors/warnings/infos - 2 errors/warnings - 1 errors - 0 none
+#define LOG_ASTAR 1
+#define LOG_TENDEUR 1
+
+
+
+class SimplePoint
+{
+ public:
+ SimplePoint(float x, float y) : x(x),y(y) {}
+ SimplePoint(const SimplePoint &p) : x(p.x),y(p.y) {}
+ bool operator!=(SimplePoint& p) {return x!=p.x||y!=p.y;}
+ virtual ~SimplePoint() {}
+ float x,y;
+};
+
+class Map
+{
+ public:
+ Map();
+ ~Map();
+ void build();
+
+ int getHeight(float x, float y);
+
+ // mpc : metre par case, par defaut chaque case fait 10cm
+ // Position en mm !!
+ char AStar(float x, float y, float goal_x, float goal_y, float mpc=100);
+
+
+ std::vector<SimplePoint> path;
+ std::vector<Obstacle*> obstacles;
+ private:
+};
+
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Obj_clap.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,59 @@
+#include "Obj_clap.h"
+
+extern Asservissement *asserv;
+
+extern bool interruption;
+
+Obj_clap::Obj_clap(float x, float y, float theta, float x2, float y2, float theta2, AX12 *ax12_brasG, AX12 *ax12_brasD)
+:Objectif(x,y,theta)
+{
+ this->ax12_brasG = ax12_brasG;
+ this->ax12_brasD = ax12_brasD;
+ this->x2 = x2;
+ this->y2 = y2;
+ this->theta2 = theta2;
+}
+
+void Obj_clap::run()
+{
+ if(theta2 == PI/2)
+ {
+ if(!interruption)
+ {
+ ax12_brasD->setGoal(BRASD_OUVERT);
+ wait(1);
+ }
+ if(!interruption) asserv->setGoal(x2,y2,theta2);
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+
+ ax12_brasD->setGoal(BRASD_FERME);
+
+ if(!interruption) wait(1);
+ }
+ else
+ {
+ if(!interruption)
+ {
+ ax12_brasG->setGoal(BRASG_OUVERT);
+ wait(1);
+ }
+ if(!interruption) asserv->setGoal(x2,y2,theta2);
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+
+ ax12_brasG->setGoal(BRASG_FERME);
+ if(!interruption) wait(1);
+ }
+
+ done = true;
+}
+
+int Obj_clap::isActive()
+{
+ if(!active)
+ return false;
+
+ if(ax12_brasG->getGoal() == BRASG_OUVERT || ax12_brasD->getGoal() == BRASD_OUVERT)
+ return false;
+
+ return true;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Obj_clap.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,21 @@
+#ifndef OBJ_CLAP_H
+#define OBJ_CLAP_H
+
+#include "AX12.h"
+#include "Objectif.h"
+
+class Obj_clap: public Objectif
+{
+public:
+ Obj_clap(float x, float y, float theta, float x2, float y2, float theta2, AX12 *ax12_brasG, AX12 *ax12_brasD);
+ virtual void run();
+ virtual int isActive();
+private:
+ AX12 *ax12_brasG;
+ AX12 *ax12_brasD;
+ float x2,y2,theta2;
+};
+
+
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Obj_depot.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,42 @@
+#include "Obj_depot.h"
+
+extern Motor motorL;
+extern Motor motorR;
+extern bool interruption;
+
+Obj_depot::Obj_depot(float x, float y, float theta, AX12 *ax12_pince)
+:Objectif(x,y,theta)
+{
+ this->ax12_pince = ax12_pince;
+ desactivate();
+}
+
+void Obj_depot::run()
+{
+ ax12_pince->setMaxTorque(MAX_TORQUE);
+ wait(0.1);
+ ax12_pince->setGoal(PINCE_OUVERTE);
+ wait(1);
+
+ if(!interruption)
+ {
+ motorL.setSpeed(-0.3);
+ motorR.setSpeed(-0.3);
+ wait(0.5);
+ motorL.setSpeed(0);
+ motorR.setSpeed(0);
+ }
+
+ done = true;
+}
+
+int Obj_depot::isActive()
+{
+ if(!active)
+ return false;
+
+ if(ax12_pince->getGoal() == PINCE_OUVERTE)
+ return false;
+
+ return true;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Obj_depot.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,19 @@
+#ifndef OBJ_DEPOT_H
+#define OBJ_DEPOT_H
+
+#include "AX12.h"
+#include "Objectif.h"
+
+class Obj_depot: public Objectif
+{
+public:
+ Obj_depot(float x, float y, float theta, AX12 *ax12_pince);
+ virtual void run();
+ virtual int isActive();
+private:
+ AX12 *ax12_pince;
+};
+
+
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Obj_pince.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,64 @@
+#include "Obj_pince.h"
+#include <vector>
+#include "Map.h"
+
+extern Asservissement *asserv;
+
+extern Motor motorL;
+extern Motor motorR;
+extern Odometry odometry;
+extern std::vector<Objectif*> objectifs;
+extern Map terrain;
+extern bool interruption;
+
+Obj_pince::Obj_pince(float x, float y, float xp, float yp, float theta, AX12 *ax12_pince)
+:Objectif(x,y,theta)
+{
+ this->ax12_pince = ax12_pince;
+ this->xp = xp;
+ this->yp = yp;
+}
+
+void Obj_pince::run()
+{
+ //if(x!=xp && y!=yp)
+ //{
+ if(!interruption) asserv->setGoal(xp,yp,theta); //On avance jusqu'au goblet/spot
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+ //}
+ ax12_pince->setMaxTorque(MAX_TORQUE);
+ wait(0.1);
+ ax12_pince->setGoal(PINCE_FERMEE);
+ wait(1.5);
+
+ done = true;
+
+ for(unsigned int i=0;i < objectifs.size();i++)
+ {
+ if(IDO_PC1 <= id && id <= IDO_PC5 && objectifs[i]->getId() == IDO_DEPOT_PC && !objectifs[i]->isDone())
+ {
+ objectifs[i]->activate();
+ break;
+ }
+ if(IDO_P1 <= id && id <= IDO_P16 && objectifs[i]->getId() == IDO_DEPOT_P && !objectifs[i]->isDone())
+ {
+ objectifs[i]->activate();
+ break;
+ }
+ }
+
+ for(unsigned int i=0;i < terrain.obstacles.size();i++)
+ if(id == terrain.obstacles[i]->getId())
+ terrain.obstacles[i]->desactivate();
+}
+
+int Obj_pince::isActive()
+{
+ if(!active)
+ return false;
+
+ if(ax12_pince->getGoal() == PINCE_FERMEE)
+ return false;
+
+ return true;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Obj_pince.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,20 @@
+#ifndef OBJ_PINCE_H
+#define OBJ_PINCE_H
+
+#include "AX12.h"
+#include "Objectif.h"
+
+class Obj_pince: public Objectif
+{
+public:
+ Obj_pince(float x, float y, float xp, float yp, float theta, AX12 *ax12_pince);
+ virtual void run();
+ virtual int isActive();
+private:
+ AX12 *ax12_pince;
+ float xp, yp;
+};
+
+
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Obj_recalage.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,63 @@
+#include "Obj_recalage.h"
+
+extern Motor motorL;
+extern Motor motorR;
+extern Odometry odometry;
+extern bool interruption;
+
+Obj_recalage::Obj_recalage(float x, float y, float theta)
+:Objectif(x,y,theta)
+{
+
+}
+
+void Obj_recalage::run()
+{
+ motorL.setSpeed(-0.3);
+ motorR.setSpeed(-0.3);
+
+ float lastPos = 0;
+ if(theta == -PI/2)
+ {
+ lastPos = odometry.getY();
+ while(abs(lastPos-odometry.getY()) > 2) // tant qu'on bouge
+ {
+ lastPos = odometry.getY();
+ }
+ // On a finit
+ if(abs((3000-82.2)-odometry.getY()) < 200) // bonne erreur, on prend en compte
+ {
+ odometry.setY(3000-82.2f);
+ odometry.setTheta(theta);
+ }
+ }
+
+ if(theta == PI/2)
+ {
+ lastPos = odometry.getY();
+ while(abs(lastPos-odometry.getY()) > 2) // tant qu'on bouge
+ {
+ lastPos = odometry.getY();
+ }
+ // On a finit
+ if(abs(82.2f-odometry.getY()) < 200.0f) // bonne erreur, on prend en compte
+ {
+ odometry.setY(82.2f);
+ odometry.setTheta(theta);
+ }
+ }
+
+ motorL.setSpeed(0);
+ motorR.setSpeed(0);
+
+ wait(0.1);
+
+ done = true;
+}
+
+int Obj_recalage::isActive()
+{
+ if(!active)
+ return false;
+ return true;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Obj_recalage.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,17 @@
+#ifndef OBJ_RECALAGE_H
+#define OBJ_RECALAGE_H
+
+#include "Objectif.h"
+
+class Obj_recalage: public Objectif
+{
+public:
+ Obj_recalage(float x, float y, float theta);
+ virtual void run();
+ virtual int isActive();
+private:
+};
+
+
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Objectif.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,11 @@
+#include "Objectif.h"
+
+Objectif::Objectif(float x, float y, float theta)
+{
+ this->x = x;
+ this->y = y;
+ this->theta = theta;
+ done = false;
+ active = true;
+ id = -1;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Objectifs/Objectif.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,29 @@
+#ifndef OBJECTIF_H
+#define OBJECTIF_H
+
+#include "includes.h"
+
+class Objectif
+{
+public:
+ Objectif(float x, float y, float theta);
+
+ virtual void run() = 0;
+ bool isDone(){return done;}
+ float getX(){return x;}
+ float getY(){return y;}
+ float getTheta(){return theta;}
+
+ int getId(){return id;}
+ void setId(int id){this->id = id;}
+
+ virtual int isActive(){return active;}
+ void activate(){active=true;}
+ void desactivate(){active=false;}
+protected:
+ float x,y,theta;
+ bool done,active;
+ int id;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Obstacles/Obs_circle.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,30 @@
+#include "Obs_circle.h"
+
+Obs_circle::Obs_circle(float robotRadius, int id, float x, float y, float size):Obstacle(robotRadius,id)
+{
+ this->x = x;
+ this->y = y;
+ this->size2 = size*size;
+}
+
+int Obs_circle::height(float x, float y)
+{
+ if(!active)
+ return 0;
+
+ float d = (x-this->x)*(x-this->x) + (y-this->y)*(y-this->y);
+ if(d <= size2) // On est dans le cercle
+ {
+ return 32000; // Interdit
+ }
+ else if(bigShape && d <= size2+robotRadius*robotRadius) // On est dans le grand cercle
+ {
+ if(!smoothBigShape)
+ return 32000; // Interdit
+ else
+ {
+ return 32000; // Interdit
+ }
+ }
+ return 0;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Obstacles/Obs_circle.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,17 @@
+#ifndef OBS_CIRCLE_H_
+#define OBS_CIRCLE_H_
+
+#include "Obstacle.h"
+
+class Obs_circle: public Obstacle
+{
+ public:
+ Obs_circle(float robotRadius, int id, float x, float y, float size);
+ virtual int height(float x, float y);
+
+ //void setPos(float x, float y);
+ private:
+ float x,y,size2; // size2 c'est la taille au carré
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Obstacles/Obs_rect.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,26 @@
+#include "Obs_rect.h"
+
+Obs_rect::Obs_rect(float robotRadius, int id, float x1, float y1, float x2, float y2):Obstacle(robotRadius,id)
+{
+ this->x1 = (x1<x2) ? x1:x2;
+ this->x2 = (x1<x2) ? x2:x1;
+ this->y1 = (y1<y2) ? y1:y2;
+ this->y2 = (y1<y2) ? y2:y1;
+}
+
+int Obs_rect::height(float x, float y)
+{
+ if(!active)
+ return 0;
+ if(bigShape)
+ {
+ if(x1 - robotRadius < x && x < x2 + robotRadius && y1 - robotRadius < y && y < y2 + robotRadius)
+ return 32000; // Dans le grand rctangle
+ }
+ else
+ {
+ if(x1 < x && x < x2 && y1 < y && y < y2)
+ return 32000; // Dans le petit rectangle
+ }
+ return 0;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Obstacles/Obs_rect.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,16 @@
+#ifndef OBS_RECT_H_
+#define OBS_RECT_H_
+
+#include "Obstacle.h"
+
+class Obs_rect: public Obstacle
+{
+ public:
+ Obs_rect(float robotRadius, int id, float x1, float y1, float x2, float y2);
+ virtual int height(float x, float y);
+
+ private:
+ float x1,y1,x2,y2;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Obstacles/Obs_robot.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,13 @@
+#include "Obs_robot.h"
+
+Obs_robot::Obs_robot(float robotRadius, int id, float x, float y, float size, float temps):Obs_circle(robotRadius,id,x,y,size)
+{
+ this->temps = temps;
+}
+
+void Obs_robot::update(float dt)
+{
+ temps -= dt;
+ if(temps <= 0)
+ desactivate();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Obstacles/Obs_robot.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,16 @@
+#ifndef OBS_ROBOT_H_
+#define OBS_ROBOT_H_
+
+#include "Obs_circle.h"
+
+class Obs_robot: public Obs_circle
+{
+ public:
+ Obs_robot(float robotRadius, int id, float x, float y, float size, float temps);
+ //virtual int height(float x, float y);
+ virtual void update(float dt);
+ private:
+ float temps;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Obstacles/Obstacle.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,15 @@
+#include "Obstacle.h"
+
+Obstacle::Obstacle(float robotRadius, int id)
+{
+ this->robotRadius = robotRadius;
+ this->id = id;
+ this->active = true;
+ bigShape = true;
+ smoothBigShape = false;
+}
+
+Obstacle::~Obstacle()
+{
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Obstacles/Obstacle.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,36 @@
+#ifndef OBSTACLE_H_
+#define OBSTACLE_H_
+
+class Obstacle
+{
+ public:
+ Obstacle(float robotRadius,int id);
+ virtual ~Obstacle();
+
+ virtual int height(float x, float y) = 0;
+
+ void setBigShape(bool bs) {bigShape = bs;}
+ bool isBigShape() {return bigShape;}
+
+ void setSmoothBigShape(bool sbs) {smoothBigShape = sbs;}
+ bool isSmoothBigShape() {return smoothBigShape;}
+
+ void setRobotRadius(float robotRadius) {this->robotRadius = robotRadius;}
+ float getRobotRadius() {return robotRadius;}
+
+ int getId(){return id;}
+ void setId(int id){this->id = id;}
+
+ void activate(){active=true;}
+ void desactivate(){active=false;}
+
+ virtual void update(float dt){}
+ protected:
+ bool bigShape,smoothBigShape;
+ bool active;
+ float robotRadius;
+
+ int id;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Map/Point.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,78 @@
+#ifndef POINT_H
+#define POINT_H
+
+#include <vector>
+
+class Point;
+
+class Point
+{
+ public:
+ Point()
+ {
+ x=y=G=H=0;
+ }
+
+ Point(int x, int y, float G=0, float H=0) : x(x),y(y),G(G),H(H)
+ {
+
+ }
+
+ virtual ~Point()
+ {
+
+ }
+
+ Point operator=(const Point &acase)
+ {
+ x=acase.x;
+ y=acase.y;
+ G=acase.G;
+ H=acase.H;
+ p=acase.p;
+ return *this;
+ }
+
+ void setx(int xx) { x=xx; }
+ void sety(int yy) { y=yy; }
+ void setG(float GG) { G=GG; }
+ void setH(float HH) { H=HH; }
+ void setParent(Point *pp) { p=pp; }
+ void setParent() { p=0; }
+
+ int getx() { return x; }
+ int gety() { return y; }
+ float getF() { return G+H; }
+ float getG() { return G; }
+ float getH() { return H; }
+ Point* getParent() { return p; }
+
+
+ bool in(std::vector<Point*> &list, unsigned int &pos)
+ {
+ for(unsigned int i=0;i<list.size();i++)
+ if(list[i]->getx() == this->getx() && list[i]->gety() == this->gety())
+ {
+ pos = i;
+ return true;
+ }
+ return false;
+ }
+
+ bool in(std::vector<Point*> &list)
+ {
+ for(unsigned int i=0;i<list.size();i++)
+ if(list[i]->getx() == this->getx() && list[i]->gety() == this->gety())
+ return true;
+ return false;
+ }
+
+ private:
+ int x;
+ int y;
+ float G;
+ float H;
+ Point *p;
+};
+
+#endif // POINT_H
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Odometry/Odometry.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,80 @@
+/**
+ * @author BERTELONE Benjamin
+ *
+ * @section DESCRIPTION
+ *
+ */
+
+#include "Odometry.h"
+#include "defines.h"
+
+extern Serial logger;
+
+
+Odometry::Odometry(QEI *qei_left, QEI *qei_right, float radius_left, float radius_right, float v)
+{
+ m_qei_left = qei_left;
+ m_qei_right = qei_right;
+ m_v = v;
+
+ m_distPerTick_left = radius_left/qei_left->getPulsesPerRev()*2*PI;
+ m_distPerTick_right = radius_right/qei_right->getPulsesPerRev()*2*PI;
+
+ m_pulses_left = qei_left->getPulses();
+ m_pulses_right = qei_right->getPulses();
+
+
+ // Vitesse du moteur gauche et droit
+ m_vitLeft = 0;
+ m_vitRight = 0;
+}
+
+void Odometry::setPos(float x, float y, float theta)
+{
+ this->x = x;
+ this->y = y;
+ this->theta = theta;
+}
+
+void Odometry::setX(float x)
+{
+ this->x = x;
+}
+
+void Odometry::setY(float y)
+{
+ this->y = y;
+}
+
+void Odometry::setTheta(float theta)
+{
+ this->theta = theta;
+}
+
+void Odometry::update(float dt)
+{
+ int delta_left = m_qei_left->getPulses() - m_pulses_left;
+ m_pulses_left = m_qei_left->getPulses();
+ int delta_right = m_qei_right->getPulses() - m_pulses_right;
+ m_pulses_right = m_qei_right->getPulses();
+
+ m_distLeft = m_pulses_left*m_distPerTick_left;
+ m_distRight = m_pulses_right*m_distPerTick_right;
+
+ m_vitLeft = m_distPerTick_left*delta_left/dt;
+ m_vitRight = m_distPerTick_right*delta_right/dt;
+
+ float deltaS = (m_distPerTick_left*delta_left + m_distPerTick_right*delta_right) / 2.0f;
+ float deltaTheta = atan((m_distPerTick_right*delta_right - m_distPerTick_left*delta_left) / m_v);
+
+ float dx = deltaS*cos(theta);
+ float dy = deltaS*sin(theta);
+
+ x += dx;
+ y += dy;
+ theta += deltaTheta;
+
+ while(theta > PI) theta -= 2*PI;
+ while(theta <= -PI) theta += 2*PI;
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Odometry/Odometry.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,48 @@
+#ifndef ODOMETRY2_H
+#define ODOMETRY2_H
+
+#include "mbed.h"
+#include "QEI.h"
+
+class Odometry
+{
+ public:
+ Odometry(QEI *qei_left, QEI *qei_right, float radius_left, float radius_right, float v);
+
+ void setPos(float x, float y, float theta);
+ void setX(float x);
+ void setY(float y);
+ void setTheta(float theta);
+
+ float getX() {return x;}
+ float getY() {return y;}
+ float getTheta() {return theta;} // ]-PI;PI]
+
+ float getVitLeft() {return m_vitLeft;}
+ float getVitRight() {return m_vitRight;}
+
+ float getDistLeft() {return m_distLeft;}
+ float getDistRight() {return m_distRight;}
+
+ void setDistLeft(float dist) {m_distLeft = dist;}
+ void setDistRight(float dist) {m_distRight = dist;}
+
+ void update(float dt);
+
+ int getPulsesLeft(void) {return m_pulses_left;}
+ int getPulsesRight(void) {return m_pulses_right;}
+
+ private:
+ QEI* m_qei_left;
+ int m_pulses_left;
+ QEI* m_qei_right;
+ int m_pulses_right;
+
+ float x, y, theta;
+ float m_vitLeft, m_vitRight;
+ float m_distLeft, m_distRight;
+
+ float m_distPerTick_left, m_distPerTick_right, m_v;
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Odometry/QEI/QEI.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,289 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * 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.
+ *
+ * @section DESCRIPTION
+ *
+ * Quadrature Encoder Interface.
+ *
+ * A quadrature encoder consists of two code tracks on a disc which are 90
+ * degrees out of phase. It can be used to determine how far a wheel has
+ * rotated, relative to a known starting position.
+ *
+ * Only one code track changes at a time leading to a more robust system than
+ * a single track, because any jitter around any edge won't cause a state
+ * change as the other track will remain constant.
+ *
+ * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
+ * and paper code tracks consisting of alternating black and white sections;
+ * alternatively, complete disk and PCB emitter/receiver encoder systems can
+ * be bought, but the interface, regardless of implementation is the same.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | ^ | | | | |
+ * ---+ ^ +-----+ +-----+ +-----
+ * ^ ^
+ * ^ +-----+ +-----+ +-----+
+ * Channel B ^ | | | | | |
+ * ------+ +-----+ +-----+ +-----
+ * ^ ^
+ * ^ ^
+ * 90deg
+ *
+ * The interface uses X2 encoding by default which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * channel A.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 ...
+ *
+ * This interface can also use X4 encoding which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * either channel.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 6 7 8 9 ...
+ *
+ * It defaults
+ *
+ * An optional index channel can be used which determines when a full
+ * revolution has occured.
+ *
+ * If a 4 pules per revolution encoder was used, with X4 encoding,
+ * the following would be observed.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ +--+ ^ ^ +--+ ^
+ * ^ ^ ^ | | ^ ^ | | ^
+ * Index ------------+ +--------+ +-----------
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 6 7 8 9 ...
+ * Rev. count 0 1 2
+ *
+ * Rotational position in degrees can be calculated by:
+ *
+ * (pulse count / X * N) * 360
+ *
+ * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
+ * of pulses per revolution.
+ *
+ * Linear position can be calculated by:
+ *
+ * (pulse count / X * N) * (1 / PPI)
+ *
+ * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
+ * pulses per revolution, and PPI is pulses per inch, or the equivalent for
+ * any other unit of displacement. PPI can be calculated by taking the
+ * circumference of the wheel or encoder disk and dividing it by the number
+ * of pulses per revolution.
+ */
+
+/**
+ * Includes
+ */
+#include "QEI.h"
+
+QEI::QEI(PinName channelA,
+ PinName channelB,
+ PinName index,
+ int pulsesPerRev,
+ Encoding encoding) : channelA_(channelA), channelB_(channelB),
+ index_(index) {
+
+ pulses_ = 0;
+ revolutions_ = 0;
+ pulsesPerRev_ = pulsesPerRev;
+ encoding_ = encoding;
+
+ //Workout what the current state is.
+ int chanA = channelA_.read();
+ int chanB = channelB_.read();
+
+ //2-bit state.
+ currState_ = (chanA << 1) | (chanB);
+ prevState_ = currState_;
+
+ //X2 encoding uses interrupts on only channel A.
+ //X4 encoding uses interrupts on channel A,
+ //and on channel B.
+ channelA_.rise(this, &QEI::encode);
+ channelA_.fall(this, &QEI::encode);
+
+ //If we're using X4 encoding, then attach interrupts to channel B too.
+ if (encoding == X4_ENCODING) {
+ channelB_.rise(this, &QEI::encode);
+ channelB_.fall(this, &QEI::encode);
+ }
+ //Index is optional.
+ if (index != NC) {
+ index_.rise(this, &QEI::index);
+ }
+
+}
+
+void QEI::reset(void) {
+
+ pulses_ = 0;
+ revolutions_ = 0;
+
+}
+
+int QEI::getCurrentState(void) {
+
+ return currState_;
+
+}
+
+int QEI::getPulses(void) {
+
+ return pulses_;
+
+}
+
+int QEI::getRevolutions(void) {
+
+ return revolutions_;
+
+}
+
+// +-------------+
+// | X2 Encoding |
+// +-------------+
+//
+// When observing states two patterns will appear:
+//
+// Counter clockwise rotation:
+//
+// 10 -> 01 -> 10 -> 01 -> ...
+//
+// Clockwise rotation:
+//
+// 11 -> 00 -> 11 -> 00 -> ...
+//
+// We consider counter clockwise rotation to be "forward" and
+// counter clockwise to be "backward". Therefore pulse count will increase
+// during counter clockwise rotation and decrease during clockwise rotation.
+//
+// +-------------+
+// | X4 Encoding |
+// +-------------+
+//
+// There are four possible states for a quadrature encoder which correspond to
+// 2-bit gray code.
+//
+// A state change is only valid if of only one bit has changed.
+// A state change is invalid if both bits have changed.
+//
+// Clockwise Rotation ->
+//
+// 00 01 11 10 00
+//
+// <- Counter Clockwise Rotation
+//
+// If we observe any valid state changes going from left to right, we have
+// moved one pulse clockwise [we will consider this "backward" or "negative"].
+//
+// If we observe any valid state changes going from right to left we have
+// moved one pulse counter clockwise [we will consider this "forward" or
+// "positive"].
+//
+// We might enter an invalid state for a number of reasons which are hard to
+// predict - if this is the case, it is generally safe to ignore it, update
+// the state and carry on, with the error correcting itself shortly after.
+void QEI::encode(void) {
+
+ int change = 0;
+ int chanA = channelA_.read();
+ int chanB = channelB_.read();
+
+ //2-bit state.
+ currState_ = (chanA << 1) | (chanB);
+
+ if (encoding_ == X2_ENCODING) {
+
+ //11->00->11->00 is counter clockwise rotation or "forward".
+ if ((prevState_ == 0x3 && currState_ == 0x0) ||
+ (prevState_ == 0x0 && currState_ == 0x3)) {
+
+ pulses_++;
+
+ }
+ //10->01->10->01 is clockwise rotation or "backward".
+ else if ((prevState_ == 0x2 && currState_ == 0x1) ||
+ (prevState_ == 0x1 && currState_ == 0x2)) {
+
+ pulses_--;
+
+ }
+
+ } else if (encoding_ == X4_ENCODING) {
+
+ //Entered a new valid state.
+ if (((currState_ ^ prevState_) != INVALID) && (currState_ != prevState_)) {
+ //2 bit state. Right hand bit of prev XOR left hand bit of current
+ //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
+ change = (prevState_ & PREV_MASK) ^ ((currState_ & CURR_MASK) >> 1);
+
+ if (change == 0) {
+ change = -1;
+ }
+
+ pulses_ -= change;
+ }
+
+ }
+
+ prevState_ = currState_;
+
+}
+
+void QEI::index(void) {
+
+ revolutions_++;
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Odometry/QEI/QEI.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,246 @@
+/**
+ * @author Aaron Berk
+ *
+ * @section LICENSE
+ *
+ * Copyright (c) 2010 ARM Limited
+ *
+ * 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.
+ *
+ * @section DESCRIPTION
+ *
+ * Quadrature Encoder Interface.
+ *
+ * A quadrature encoder consists of two code tracks on a disc which are 90
+ * degrees out of phase. It can be used to determine how far a wheel has
+ * rotated, relative to a known starting position.
+ *
+ * Only one code track changes at a time leading to a more robust system than
+ * a single track, because any jitter around any edge won't cause a state
+ * change as the other track will remain constant.
+ *
+ * Encoders can be a homebrew affair, consisting of infrared emitters/receivers
+ * and paper code tracks consisting of alternating black and white sections;
+ * alternatively, complete disk and PCB emitter/receiver encoder systems can
+ * be bought, but the interface, regardless of implementation is the same.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | ^ | | | | |
+ * ---+ ^ +-----+ +-----+ +-----
+ * ^ ^
+ * ^ +-----+ +-----+ +-----+
+ * Channel B ^ | | | | | |
+ * ------+ +-----+ +-----+ +-----
+ * ^ ^
+ * ^ ^
+ * 90deg
+ *
+ * The interface uses X2 encoding by default which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * channel A.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 ...
+ *
+ * This interface can also use X4 encoding which calculates the pulse count
+ * based on reading the current state after each rising and falling edge of
+ * either channel.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 6 7 8 9 ...
+ *
+ * It defaults
+ *
+ * An optional index channel can be used which determines when a full
+ * revolution has occured.
+ *
+ * If a 4 pules per revolution encoder was used, with X4 encoding,
+ * the following would be observed.
+ *
+ * +-----+ +-----+ +-----+
+ * Channel A | | | | | |
+ * ---+ +-----+ +-----+ +-----
+ * ^ ^ ^ ^ ^
+ * ^ +-----+ ^ +-----+ ^ +-----+
+ * Channel B ^ | ^ | ^ | ^ | ^ | |
+ * ------+ ^ +-----+ ^ +-----+ +--
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * ^ ^ ^ +--+ ^ ^ +--+ ^
+ * ^ ^ ^ | | ^ ^ | | ^
+ * Index ------------+ +--------+ +-----------
+ * ^ ^ ^ ^ ^ ^ ^ ^ ^ ^
+ * Pulse count 0 1 2 3 4 5 6 7 8 9 ...
+ * Rev. count 0 1 2
+ *
+ * Rotational position in degrees can be calculated by:
+ *
+ * (pulse count / X * N) * 360
+ *
+ * Where X is the encoding type [e.g. X4 encoding => X=4], and N is the number
+ * of pulses per revolution.
+ *
+ * Linear position can be calculated by:
+ *
+ * (pulse count / X * N) * (1 / PPI)
+ *
+ * Where X is encoding type [e.g. X4 encoding => X=44], N is the number of
+ * pulses per revolution, and PPI is pulses per inch, or the equivalent for
+ * any other unit of displacement. PPI can be calculated by taking the
+ * circumference of the wheel or encoder disk and dividing it by the number
+ * of pulses per revolution.
+ */
+
+#ifndef QEI_H
+#define QEI_H
+
+/**
+ * Includes
+ */
+#include "mbed.h"
+
+/**
+ * Defines
+ */
+#define PREV_MASK 0x1 //Mask for the previous state in determining direction
+//of rotation.
+#define CURR_MASK 0x2 //Mask for the current state in determining direction
+//of rotation.
+#define INVALID 0x3 //XORing two states where both bits have changed.
+
+/**
+ * Quadrature Encoder Interface.
+ */
+class QEI {
+
+public:
+
+ typedef enum Encoding {
+
+ X2_ENCODING,
+ X4_ENCODING
+
+ } Encoding;
+
+ /**
+ * Constructor.
+ *
+ * Reads the current values on channel A and channel B to determine the
+ * initial state.
+ *
+ * Attaches the encode function to the rise/fall interrupt edges of
+ * channels A and B to perform X4 encoding.
+ *
+ * Attaches the index function to the rise interrupt edge of channel index
+ * (if it is used) to count revolutions.
+ *
+ * @param channelA mbed pin for channel A input.
+ * @param channelB mbed pin for channel B input.
+ * @param index mbed pin for optional index channel input,
+ * (pass NC if not needed).
+ * @param pulsesPerRev Number of pulses in one revolution.
+ * @param encoding The encoding to use. Uses X2 encoding by default. X2
+ * encoding uses interrupts on the rising and falling edges
+ * of only channel A where as X4 uses them on both
+ * channels.
+ */
+ QEI(PinName channelA, PinName channelB, PinName index, int pulsesPerRev, Encoding encoding = X2_ENCODING);
+
+ /**
+ * Reset the encoder.
+ *
+ * Sets the pulses and revolutions count to zero.
+ */
+ void reset(void);
+
+ /**
+ * Read the state of the encoder.
+ *
+ * @return The current state of the encoder as a 2-bit number, where:
+ * bit 1 = The reading from channel B
+ * bit 2 = The reading from channel A
+ */
+ int getCurrentState(void);
+
+ /**
+ * Read the number of pulses recorded by the encoder.
+ *
+ * @return Number of pulses which have occured.
+ */
+ int getPulses(void);
+
+ /**
+ * Read the number of revolutions recorded by the encoder on the index channel.
+ *
+ * @return Number of revolutions which have occured on the index channel.
+ */
+ int getRevolutions(void);
+
+
+
+ /**
+ * Update the pulse count.
+ *
+ * Called on every rising/falling edge of channels A/B.
+ *
+ * Reads the state of the channels and determines whether a pulse forward
+ * or backward has occured, updating the count appropriately.
+ */
+ void encode(void);
+
+ int getPulsesPerRev() {return pulsesPerRev_;}
+private:
+ /**
+ * Called on every rising edge of channel index to update revolution
+ * count by one.
+ */
+ void index(void);
+
+ Encoding encoding_;
+
+ InterruptIn channelA_;
+ InterruptIn channelB_;
+ InterruptIn index_;
+
+ int pulsesPerRev_;
+ int prevState_;
+ int currState_;
+
+ volatile int pulses_;
+ volatile int revolutions_;
+
+};
+
+#endif /* QEI_H */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/defines.h Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,119 @@
+#ifndef DEFINE_H
+#define DEFINE_H
+
+#define OUT_USB
+
+enum ID
+{
+ IDO_MG,
+ IDO_MH,
+ IDO_MD,
+ IDO_MB,
+ IDO_M1,
+ IDO_M2,
+ IDO_M3,
+ IDO_M4,
+ IDO_M5,
+ IDO_M6,
+ IDO_D1,
+ IDO_D2,
+ IDO_D3,
+ IDO_D4,
+ IDO_E,
+ IDO_S,
+ IDO_PC1,
+ IDO_PC2,
+ IDO_PC3,
+ IDO_PC4,
+ IDO_PC5,
+ IDO_P1,
+ IDO_P2,
+ IDO_P3,
+ IDO_P4,
+ IDO_P5,
+ IDO_P6,
+ IDO_P7,
+ IDO_P8,
+ IDO_P9,
+ IDO_P10,
+ IDO_P11,
+ IDO_P12,
+ IDO_P13,
+ IDO_P14,
+ IDO_P15,
+ IDO_P16,
+ IDO_DEPOT_PC,
+ IDO_DEPOT_P,
+ IDO_ROBOT
+};
+
+#define ROBOTRADIUS 190
+
+#define MAXPOINT 8000
+
+// ----- Loggeur ----- //
+
+#ifdef OUT_USB
+ #define OUT_TX USBTX
+ #define OUT_RX USBRX
+#else
+ #define OUT_TX PA_11
+ #define OUT_RX PA_12
+#endif
+
+// ----- Moteurs ----- //
+
+#define PWM_MOT1 PB_13
+#define PWM_MOT2 PB_14
+#define PWM_MOT3 PB_15
+
+#define DIR_MOT1 PC_9
+#define DIR_MOT2 PB_8
+#define DIR_MOT3 PB_9
+
+// ----- Odometrie ----- //
+
+#define ODO_G_B PA_10
+#define ODO_G_A PB_3
+
+#define ODO_D_B PB_5
+#define ODO_D_A PB_4
+
+#define PI 3.14159f
+#define RAYONG 63.84f/2.0f
+#define RAYOND 63.65f/2.0f
+#define ENTREAXE 252.0f
+
+// ----- Boutons ----- //
+
+#define LED_DESSUS PH_1
+#define BP_DESSUS PC_8
+#define TIRETTE_DESSUS PC_6
+#define COULEUR_DESSUS PC_5
+
+#define COULEUR_JAUNE 0
+#define COULEUR_VERTE 1
+
+// ----- AX12 ----- //
+
+#define AX12_TX PA_9
+#define AX12_RX NC
+
+#define MAX_TORQUE 500
+
+#define BRASG_OUVERT 60
+#define BRASG_FERME 155
+#define BRASD_OUVERT 240
+#define BRASD_FERME 145
+
+#define PINCE_OUVERTE 80
+#define PINCE_FERMEE 3
+
+// ----- Sharp ----- //
+
+#define SHARP_D A3
+#define SHARP_DG A2
+#define SHARP_DD A4
+#define SHARP_A A1
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/includes.h Sun May 24 12:30:47 2015 +0000 @@ -0,0 +1,10 @@ +#ifndef INCLUDE_H +#define INCLUDE_H + +#include "mbed.h" +#include "defines.h" +#include "Odometry.h" +#include "Asservissement_v1.h" +#include "Motor.h" + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sun May 24 12:30:47 2015 +0000
@@ -0,0 +1,525 @@
+#include "includes.h"
+
+#include "Map.h"
+
+#include "Objectif.h"
+#include "Obj_clap.h"
+#include "Obj_pince.h"
+#include "Obj_depot.h"
+#include "Obj_recalage.h"
+
+#include "Obs_robot.h"
+
+#include "AX12.h"
+
+
+void update();
+void update_odo();
+
+// *--------------------------* //
+// Déclarations //
+
+// Decl. logger //
+
+Serial logger(OUT_TX, OUT_RX); // tx, rx
+
+// Decl. timer //
+
+Timer t;
+Ticker ticker;
+
+// Decl. AX12 //
+
+AX12 ax12_pince(AX12_TX,AX12_RX,5,250000);
+AX12 ax12_brasG(AX12_TX,AX12_RX,2,250000);
+AX12 ax12_brasD(AX12_TX,AX12_RX,3,250000);
+
+// Decl. Moteurs //
+
+PwmOut pw1(PWM_MOT1);
+DigitalOut dir1(DIR_MOT1);
+PwmOut pw2(PWM_MOT2);
+DigitalOut dir2(DIR_MOT2);
+
+Motor motorL(PWM_MOT1,DIR_MOT1);
+Motor motorR(PWM_MOT2,DIR_MOT2);
+
+// Decl. Sharps //
+
+AnalogIn sharp_devant(SHARP_D);
+AnalogIn sharp_devant_droite(SHARP_DD);
+AnalogIn sharp_devant_gauche(SHARP_DG);
+AnalogIn sharp_arriere(SHARP_A);
+
+// Decl. Boutons //
+
+DigitalIn bp(BP_DESSUS);
+DigitalIn tirette(TIRETTE_DESSUS);
+DigitalIn couleur(COULEUR_DESSUS);
+
+// Decl. Odometrie //
+
+QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
+QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
+
+Odometry odometry(&qei_left,&qei_right,RAYONG,RAYOND,ENTREAXE);
+
+// Decl. Asserv //
+
+Asservissement *asserv = new Asservissement_v1(odometry,motorL,motorR);
+
+// Decl. IA //
+
+Map terrain;
+Timer timer;
+Timeout timeout;
+bool fini = false;
+bool interruption = false;
+std::vector<Objectif*> objectifs;
+char couleurRobot = COULEUR_JAUNE;
+
+void stop();
+
+// Fin Decl. //
+// *--------------------------* //
+
+
+int main()
+{
+ #ifdef OUT_USB
+ logger.baud((int)921600);
+ #endif
+
+ // *--------------------------* //
+ // Init //
+
+ logger.printf("[00.000] Initialisation.............");
+
+ // Init. AX12 //
+
+ ax12_pince.setMode(0);
+ ax12_brasG.setMode(0);
+ ax12_brasD.setMode(0);
+
+ // Init. Moteurs //
+
+ motorL.setSpeed(0);
+ motorR.setSpeed(0);
+
+ // Init. Sharps //
+
+ // Init. Boutons //
+
+ // Init. Odometrie //
+
+
+ odometry.setTheta(PI/2);
+ odometry.setX(0);
+ odometry.setY(0);
+
+ // Init. IA //
+
+ terrain.build();
+
+ logger.printf("[done]\r\n");
+
+ // Fin Init. //
+ // *--------------------------* //
+
+ // *--------------------------* //
+ // MIP //
+
+ logger.printf("Appuyer sur le bouton pour mettre en position\r\n");
+
+ while(!bp); // On attend le top de mise en position
+
+ // *--------------------------* //
+ // Asserv //
+
+ logger.printf("[00.000] Demarrage asserv!...........");
+ t.start();
+ ticker.attach_us(&update,10000); //100Hz/10ms
+ logger.printf("[done]\r\n");
+
+ timer.reset();
+ timer.start();
+
+ while(1);
+
+ // //
+ // *--------------------------* //
+
+ logger.printf("[00.000] MIP........................");
+
+ if(couleur == COULEUR_JAUNE)
+ {
+ couleurRobot = COULEUR_JAUNE;
+
+ odometry.setTheta(PI/2);
+ odometry.setX(1000);
+ odometry.setY(157);
+
+ asserv->setGoal(1000,400);
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+
+ objectifs.push_back( new Obj_clap(1750, 650, PI/2, 1750, 880, PI/2, &ax12_brasG, &ax12_brasD) );
+
+ objectifs.push_back( new Obj_pince(1750, 250+350, 1750, 250+190, -PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_PC2);
+
+ objectifs.push_back( new Obj_depot(1000, 350+1*100, -PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_PC);
+
+ objectifs.push_back( new Obj_depot(1000, (350+2*100), -PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_P);
+ objectifs.push_back( new Obj_depot(1100, (350+3*100), -PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_P);
+
+ objectifs.push_back( new Obj_pince(1355-310, (870), 1355-170, (870), 0, &ax12_pince) );
+ objectifs.back()->setId(IDO_P4);
+
+ objectifs.push_back( new Obj_pince(1730, 350, 1730, 240, -PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_P5);
+
+
+ objectifs.push_back( new Obj_clap(1750, 3000-2776, -PI/4, 1750, 3000-2630, PI/2, &ax12_brasG, &ax12_brasD) );
+
+ }
+ else
+ {
+ couleurRobot = COULEUR_VERTE;
+
+ odometry.setTheta(-PI/2);
+ odometry.setX(1000);
+ odometry.setY(3000-157);
+
+ asserv->setGoal(1000,3000-400);
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+
+ objectifs.push_back( new Obj_clap(1750, 2350, -PI/2, 1750, 2100, -PI/2, &ax12_brasG, &ax12_brasD) );
+
+ objectifs.push_back( new Obj_pince(1750, 3000-(250+350), 1750, 3000-(250+190), PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_PC4);
+
+ objectifs.push_back( new Obj_depot(1000, 3000-(350+1*100), PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_PC);
+
+ objectifs.push_back( new Obj_depot(1000, 3000-(350+2*100), PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_P);
+ objectifs.push_back( new Obj_depot(1100, 3000-(350+3*100), PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_DEPOT_P);
+
+ objectifs.push_back( new Obj_pince(1355-330, 3000-(870), 1355-170, 3000-(870), 0, &ax12_pince) );
+ objectifs.back()->setId(IDO_P12);
+
+ objectifs.push_back( new Obj_pince(1730, 3000-350, 1730, 3000-240, PI/2, &ax12_pince) );
+ objectifs.back()->setId(IDO_P13);
+
+ objectifs.push_back( new Obj_clap(1750, 2776, PI/4, 1750, 2630, -PI/2, &ax12_brasG, &ax12_brasD) );
+ }
+
+
+ ax12_pince.setMaxTorque(MAX_TORQUE);
+ wait(0.1);
+ ax12_brasG.setMaxTorque(MAX_TORQUE);
+ wait(0.1);
+ ax12_brasD.setMaxTorque(MAX_TORQUE);
+ wait(0.1);
+ ax12_pince.setGoal(PINCE_FERMEE);
+ wait(0.1);
+ ax12_brasG.setGoal(BRASG_OUVERT);
+ wait(0.1);
+ ax12_brasD.setGoal(BRASD_OUVERT);
+ wait(0.5);
+ ax12_pince.setGoal(PINCE_OUVERTE);
+ wait(0.1);
+ ax12_brasG.setGoal(BRASG_FERME);
+ wait(0.1);
+ ax12_brasD.setGoal(BRASD_FERME);
+
+ logger.printf("[done]\r\n");
+
+ // //
+ // *--------------------------* //
+
+ // *--------------------------* //
+ // Tirrette + couleur //
+
+ while(tirette) // La tirrette
+ {
+ logger.printf("Point actuel : %.3f\t%.3f\t%.3f\r\n",odometry.getX(), odometry.getY(), odometry.getTheta()*180/PI);
+ wait(0.3);
+ }
+ timer.reset();
+ timer.start();
+ timeout.attach(&stop,90); // On lance le chrono de 90s
+
+ // //
+ // *--------------------------* //
+
+ // *--------------------------* //
+ // IA //
+
+ while(!fini)
+ {
+ if(terrain.getHeight(odometry.getX(), odometry.getY()) >= 32000)
+ {
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Collision, echappement.....");
+
+ // Réduction de la taille des obstacles
+ for(unsigned int i=0;i < terrain.obstacles.size();i++)
+ {
+ if(terrain.obstacles[i]->height(odometry.getX(), odometry.getY()) >= 32000)
+ logger.printf("\r\n-> %d",terrain.obstacles[i]->getId());
+ while(terrain.obstacles[i]->height(odometry.getX(), odometry.getY()) >= 32000)
+ terrain.obstacles[i]->setRobotRadius(terrain.obstacles[i]->getRobotRadius()-1);
+ }
+
+ logger.printf("[done]\r\n");
+ }
+
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Recherche d'objectif.......");
+ unsigned int objAct = 0;
+ bool newObj = false;
+ for(objAct = 0 ; objAct < objectifs.size() ; objAct++)
+ {
+ if(objectifs[objAct]->isDone()) // Pas la peine de le faire, il est déjà fait ;)
+ continue;
+ if(!objectifs[objAct]->isActive()) // Pas la peine de le faire, il n'est pas actif
+ continue;
+ int retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 10);
+ if(retourAStar == 1) // L'objectif est atteignable !!
+ {
+ newObj = true;
+ break;
+ }
+
+ if(retourAStar == 3) // Overload memoire
+ retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 25);
+
+ if(retourAStar == 1) // L'objectif est atteignable !!
+ {
+ newObj = true;
+ break;
+ }
+
+ if(retourAStar == 3) // Overload memoire
+ retourAStar = terrain.AStar(odometry.getX(), odometry.getY(), objectifs[objAct]->getX(), objectifs[objAct]->getY(), 50);
+
+ if(retourAStar == 1) // L'objectif est atteignable !!
+ {
+ newObj = true;
+ break;
+ }
+ }
+
+ logger.printf("[done]\r\n");
+
+ // On remet la taille des obstacles
+ for(unsigned int i=0;i < terrain.obstacles.size();i++)
+ terrain.obstacles[i]->setRobotRadius(ROBOTRADIUS);
+
+ if(newObj)
+ {
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("-> Objectif : %d\r\n",objectifs[objAct]->getId());
+
+ // Déplacement vers le nouvel objectif //
+
+ for(unsigned int i=1;i<terrain.path.size()-1;i++)
+ {
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Point actuel : %.3f\t%.3f\t%.3f\r\n",odometry.getX(), odometry.getY(), odometry.getTheta()*180/PI);
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Prochain point : %d\t%.3f\t%.3f\r\n",i,terrain.path[i].x,terrain.path[i].y);
+
+ asserv->setGoal(terrain.path[i].x,terrain.path[i].y);
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+ }
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Dernier point : %.3f\t%.3f\r\n",objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta()*180/PI);
+
+
+ asserv->setGoal(objectifs[objAct]->getX(),objectifs[objAct]->getY(),objectifs[objAct]->getTheta());
+ while(!asserv->isArrived() && !interruption)wait(0.1);
+
+
+ // Execution de l'objectif //
+ if(!fini && !interruption)
+ {
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Execution de l'objectif....");
+
+ objectifs[objAct]->run();
+ logger.printf("[done]\r\n");
+ }
+ }
+ else
+ {
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("-> Nothind to be done ... :D \r\n");
+
+ ax12_brasG.setMaxTorque(MAX_TORQUE);
+ ax12_brasD.setMaxTorque(MAX_TORQUE);
+
+ ax12_brasG.setGoal(155-30);
+ ax12_brasD.setGoal(BRASD_FERME);
+ wait(1);
+ ax12_brasG.setGoal(BRASG_FERME);
+ ax12_brasD.setGoal(145+30);
+ wait(1);
+ ax12_brasD.setGoal(BRASD_FERME);
+ wait(1);
+
+ ax12_brasG.setMaxTorque(0);
+ ax12_brasD.setMaxTorque(0);
+ }
+
+ interruption = false;
+ }
+
+ while(1);
+}
+
+#define MEAN 6
+
+void update()
+{
+ /*static float sharp[MEAN][3];
+ static int i_sharp = 0;
+ sharp[i_sharp][1] = sharp_devant.read()*3.3;
+ sharp[i_sharp][0] = sharp_devant_droite.read()*3.3;
+ sharp[i_sharp][2] = sharp_devant_gauche.read()*3.3;
+ i_sharp = (i_sharp+1)%MEAN;
+
+ float sharp_value[3];
+ for(unsigned int i=0;i<3;i++)
+ {
+ sharp_value[i] = 0;
+ for(unsigned int ii=0;ii<MEAN;ii++)
+ sharp_value[i] += sharp[ii][i];
+
+ sharp_value[i] = sharp_value[i] / MEAN;
+ }*/
+
+ float dt = t.read();
+ t.reset();
+
+ odometry.update(dt);
+ asserv->update(dt);
+
+ //logger.printf("%.3f|%.3f|%.3f\r\n",timer.read()*1000,odometry.getVitLeft(),odometry.getVitRight());
+
+ for(unsigned int i=0;i < terrain.obstacles.size();i++)
+ terrain.obstacles[i]->update(dt);
+
+ /*
+ #ifdef PLAN_A
+ float phi_r = (float)instanceasserv->getPhiR();
+ float phi_l = (float)instanceasserv->getPhiL();
+ float phi_max = (float)instanceasserv->getPhiMax();
+
+ motorR.setSpeed(0.08+(phi_r/phi_max));
+ motorL.setSpeed(0.08+(phi_l/phi_max));
+ #endif
+
+ bool sharpActif = true;
+ static float lastTime = -1;
+
+ // Distributeur
+ if(odometry.getX() <= 600 && ((-PI <= odometry.getTheta() && odometry.getTheta() <= -3*PI/4) || (3*PI/4 <= odometry.getTheta() && odometry.getTheta() <= PI) ))
+ sharpActif = false;
+
+ if(odometry.getX() >= 1300 && -PI/4 <= odometry.getTheta() && odometry.getTheta() <= PI/4)
+ sharpActif = false;
+
+ if(odometry.getY() <= 650 && -3*PI/4 <= odometry.getTheta() && odometry.getTheta() <= -PI/4)
+ sharpActif = false;
+
+ if(odometry.getY() >= 2300 && -3*PI/4 <= odometry.getTheta() && odometry.getTheta() <= -PI/4)
+ sharpActif = false;
+
+ if(couleur == COULEUR_JAUNE)
+ {
+ if(700 <= odometry.getX() && odometry.getX() <= 1300 && odometry.getY() <= 1000 && -3*PI/4 <= odometry.getTheta() && odometry.getTheta() <= -PI/4)
+ sharpActif = false;
+ }
+ else
+ {
+ if(700 <= odometry.getX() && odometry.getX() <= 1300 && 2000 <= odometry.getY() && -3*PI/4 <= odometry.getTheta() && odometry.getTheta() <= -PI/4)
+ sharpActif = false;
+ }
+
+ if(odometry.getX() >= 1400 && PI/4 <= odometry.getTheta() && odometry.getTheta() <= 3*PI/4)
+ sharp_value[0] = 0;
+
+ if(odometry.getX() >= 1400 && -2*PI/6 <= odometry.getTheta() && odometry.getTheta() <= 2*PI/6)
+ {
+ sharp_value[2] = 0;
+ sharp_value[1] = 0;
+ }
+
+ if(timer.read() < 5)
+ sharpActif = false;
+
+ if((sharp_value[0] >= 0.9 || sharp_value[1] >= 0.9 || sharp_value[2] >= 0.9 ) && asserv->getState() == 2 && sharpActif) // Quelque chose devant et on avance!!
+ {
+ motorL.setSpeed(0);
+ motorR.setSpeed(0);
+ if(lastTime == -1)
+ lastTime = timer.read();
+
+ if(timer.read() - lastTime > 2) //Si on est bloqué depuis 2 secondes
+ {
+ float x = odometry.getX() + 400*cos(odometry.getTheta());
+ float y = odometry.getX() + 400*sin(odometry.getTheta());
+ terrain.obstacles.push_back(new Obs_robot(ROBOTRADIUS,IDO_ROBOT,x,y,300,10));
+
+ interruption = true;
+ asserv->stop();
+
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Evitement\r\n");
+
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Point actuel : %.3f\t%.3f\t%.3f\r\n",odometry.getX(), odometry.getY(), odometry.getTheta()*180/PI);
+
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Mechant robot : %.3f\t%.3f\r\n",x, y);
+ }
+ }
+ else
+ {
+ lastTime = -1;
+ } */
+}
+
+void stop()
+{
+ fini = true;
+ interruption = true;
+ asserv->stop();
+
+ ax12_brasG.setMaxTorque(MAX_TORQUE);
+ ax12_brasD.setMaxTorque(MAX_TORQUE);
+ ax12_pince.setMaxTorque(MAX_TORQUE);
+
+ ax12_brasG.setGoal(BRASG_FERME);
+ ax12_brasD.setGoal(BRASD_FERME);
+ ax12_pince.setGoal(PINCE_OUVERTE);
+
+ logger.printf( (timer.read() < 10 ? "[0%.3f] " : "[%.3f] "), timer.read());
+ logger.printf("Fin des 90 secondes !");
+
+ wait(2);
+
+ fini = true;
+ interruption = true;
+ asserv->stop();
+
+ ax12_brasG.setMaxTorque(0);
+ ax12_brasD.setMaxTorque(0);
+ ax12_pince.setMaxTorque(0);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun May 24 12:30:47 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/dbbf35b96557 \ No newline at end of file
