Revision 9:e0904a2c1bc0, committed 2016-11-23

Comitter:

jpxu

Date:

Wed Nov 23 09:00:13 2016 +0000

Parent:

8:4b7d6ea9b35b

Commit message:

labyrinth

Changed in this revision

--- a/m3pi.cpp	Thu May 12 13:26:37 2011 +0000
+++ b/m3pi.cpp	Wed Nov 23 09:00:13 2016 +0000
@@ -20,61 +20,61 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
-
+ 
 #include "mbed.h"
 #include "m3pi.h"
-
+ 
 m3pi::m3pi(PinName nrst, PinName tx, PinName rx) :  Stream("m3pi"), _nrst(nrst), _ser(tx, rx)  {
     _ser.baud(115200);
     reset();
 }
-
+ 
 m3pi::m3pi() :  Stream("m3pi"), _nrst(p23), _ser(p9, p10)  {
     _ser.baud(115200);
     reset();
 }
-
-
+ 
+ 
 void m3pi::reset () {
     _nrst = 0;
     wait (0.01);
     _nrst = 1;
     wait (0.1);
 }
-
+ 
 void m3pi::left_motor (float speed) {
+    motor(1,speed);
+}
+ 
+void m3pi::right_motor (float speed) {
     motor(0,speed);
 }
-
-void m3pi::right_motor (float speed) {
-    motor(1,speed);
-}
-
+ 
 void m3pi::forward (float speed) {
     motor(0,speed);
     motor(1,speed);
 }
-
+ 
 void m3pi::backward (float speed) {
     motor(0,-1.0*speed);
     motor(1,-1.0*speed);
 }
-
+ 
 void m3pi::left (float speed) {
     motor(0,speed);
     motor(1,-1.0*speed);
 }
-
+ 
 void m3pi::right (float speed) {
     motor(0,-1.0*speed);
     motor(1,speed);
 }
-
+ 
 void m3pi::stop (void) {
     motor(0,0.0);
     motor(1,0.0);
 }
-
+ 
 void m3pi::motor (int motor, float speed) {
     char opcode = 0x0;
     if (speed > 0.0) {
@@ -89,11 +89,11 @@
             opcode = M2_BACKWARD;
     }
     unsigned char arg = 0x7f * abs(speed);
-
+ 
     _ser.putc(opcode);
     _ser.putc(arg);
 }
-
+ 
 float m3pi::battery() {
     _ser.putc(SEND_BATTERY_MILLIVOLTS);
     char lowbyte = _ser.getc();
@@ -101,7 +101,7 @@
     float v = ((lowbyte + (hibyte << 8))/1000.0);
     return(v);
 }
-
+ 
 float m3pi::line_position() {
     int pos = 0;
     _ser.putc(SEND_LINE_POSITION);
@@ -111,21 +111,30 @@
     float fpos = ((float)pos - 2048.0)/2048.0;
     return(fpos);
 }
-
+ 
+void m3pi::calibrated_sensors(unsigned short ltab[5]) {
+    unsigned i;
+    _ser.putc(SEND_CALIB_SENSOR_VALUES);
+    for(i=0;i<5;i++){
+        ltab[i] = (unsigned short) _ser.getc();
+        ltab[i] += _ser.getc() << 8;
+    }
+}
+ 
 char m3pi::sensor_auto_calibrate() {
     _ser.putc(AUTO_CALIBRATE);
     return(_ser.getc());
 }
-
-
+ 
+ 
 void m3pi::calibrate(void) {
     _ser.putc(PI_CALIBRATE);
 }
-
+ 
 void m3pi::reset_calibration() {
     _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
 }
-
+ 
 void m3pi::PID_start(int max_speed, int a, int b, int c, int d) {
     _ser.putc(max_speed);
     _ser.putc(a);
@@ -133,11 +142,11 @@
     _ser.putc(c);
     _ser.putc(d);
 }
-
+ 
 void m3pi::PID_stop() {
     _ser.putc(STOP_PID);
 }
-
+ 
 float m3pi::pot_voltage(void) {
     int volt = 0;
     _ser.putc(SEND_TRIMPOT);
@@ -145,25 +154,25 @@
     volt += _ser.getc() << 8;
     return(volt);
 }
-
-
+ 
+ 
 void m3pi::leds(int val) {
-
+ 
     BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13);
     _leds = val;
 }
-
-
+ 
+ 
 void m3pi::locate(int x, int y) {
     _ser.putc(DO_LCD_GOTO_XY);
     _ser.putc(x);
     _ser.putc(y);
 }
-
+ 
 void m3pi::cls(void) {
     _ser.putc(DO_CLEAR);
 }
-
+ 
 int m3pi::print (char* text, int length) {
     _ser.putc(DO_PRINT);  
     _ser.putc(length);       
@@ -172,7 +181,7 @@
     }
     return(0);
 }
-
+ 
 int m3pi::_putc (int c) {
     _ser.putc(DO_PRINT);  
     _ser.putc(0x1);       
@@ -180,24 +189,24 @@
     wait (0.001);
     return(c);
 }
-
+ 
 int m3pi::_getc (void) {
     char r = 0;
     return(r);
 }
-
+ 
 int m3pi::putc (int c) {
     return(_ser.putc(c));
 }
-
+ 
 int m3pi::getc (void) {
     return(_ser.getc());
 }
-
-
-
-
-
+ 
+ 
+ 
+ 
+ 
 #ifdef MBED_RPC
 const rpc_method *m3pi::get_rpc_methods() {
     static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<m3pi, float, &m3pi::forward> },
@@ -210,10 +219,10 @@
         { "battery", rpc_method_caller<float, m3pi, &m3pi::battery> },
         { "line_position", rpc_method_caller<float, m3pi, &m3pi::line_position> },
         { "sensor_auto_calibrate", rpc_method_caller<char, m3pi, &m3pi::sensor_auto_calibrate> },
-
-
+ 
+ 
         RPC_METHOD_SUPER(Base)
     };
     return rpc_methods;
 }
-#endif
+#endif
\ No newline at end of file

--- a/m3pi.h	Thu May 12 13:26:37 2011 +0000
+++ b/m3pi.h	Wed Nov 23 09:00:13 2016 +0000
@@ -19,19 +19,20 @@
  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  * THE SOFTWARE.
  */
-
+ 
 #ifndef M3PI_H
 #define M3PI_H
-
+ 
 #include "mbed.h"
 #include "platform.h"
-
+ 
 #ifdef MBED_RPC
 #include "rpc.h"
 #endif
-
+ 
 #define SEND_SIGNATURE 0x81
 #define SEND_RAW_SENSOR_VALUES 0x86
+#define SEND_CALIB_SENSOR_VALUES 0x87
 #define SEND_TRIMPOT 0xB0
 #define SEND_BATTERY_MILLIVOLTS 0xB1
 #define DO_PLAY 0xB3
@@ -48,24 +49,24 @@
 #define M1_BACKWARD 0xC2
 #define M2_FORWARD 0xC5
 #define M2_BACKWARD 0xC6
-
-
-
+ 
+ 
+ 
 /** m3pi control class
  *
  * Example:
  * @code
  * // Drive the m3pi forward, turn left, back, turn right, at half speed for half a second
-
+ 
    #include "mbed.h"
    #include "m3pi.h"
-
+ 
    m3pi pi;
-
+ 
    int main() {
-
+ 
      wait(0.5);
-
+ 
      pi.forward(0.5);
      wait (0.5);
      pi.left(0.5);
@@ -74,17 +75,17 @@
      wait (0.5);
      pi.right(0.5);
      wait (0.5);
-
+ 
      pi.stop();
-
+ 
  }
  * @endcode
  */
 class m3pi :  public Stream {
-
+ 
     // Public functions
 public:
-
+ 
     /** Create the m3pi object connected to the default pins
      *
      * @param nrst GPIO pin used for reset. Default is p23
@@ -92,70 +93,70 @@
      * @param rx Serial receive pin. Default is p10
      */
     m3pi();
-
-
+ 
+ 
     /** Create the m3pi object connected to specific pins
      *
      */
     m3pi(PinName nrst, PinName tx, PinName rx);
-
-
-
+ 
+ 
+ 
     /** Force a hardware reset of the 3pi
      */
     void reset (void);
-
+ 
     /** Directly control the speed and direction of the left motor
      *
      * @param speed A normalised number -1.0 - 1.0 represents the full range.
      */
     void left_motor (float speed);
-
+ 
     /** Directly control the speed and direction of the right motor
      *
      * @param speed A normalised number -1.0 - 1.0 represents the full range.
      */
     void right_motor (float speed);
-
+ 
     /** Drive both motors forward as the same speed
      *
      * @param speed A normalised number 0 - 1.0 represents the full range.
      */
     void forward (float speed);
-
+ 
     /** Drive both motors backward as the same speed
      *
      * @param speed A normalised number 0 - 1.0 represents the full range.
      */
     void backward (float speed);
-
+ 
     /** Drive left motor backwards and right motor forwards at the same speed to turn on the spot
      *
      * @param speed A normalised number 0 - 1.0 represents the full range.
      */
     void left (float speed);
-
+ 
     /** Drive left motor forward and right motor backwards at the same speed to turn on the spot
      * @param speed A normalised number 0 - 1.0 represents the full range.
      */
     void right (float speed);
-
+ 
     /** Stop both motors
      *
      */
     void stop (void);
-
+ 
     /** Read the voltage of the potentiometer on the 3pi
      * @returns voltage as a float
      *
      */
     float pot_voltage(void);
-
+ 
     /** Read the battery voltage on the 3pi
      * @returns battery voltage as a float
      */
     float battery(void);
-
+ 
     /** Read the position of the detected line
      * @returns position as A normalised number -1.0 - 1.0 represents the full range.
      *  -1.0 means line is on the left, or the line has been lost
@@ -163,74 +164,79 @@
      *   1.0 means the line is on the right
      */
     float line_position (void);
-
-
+    /** lecture capteurs calibres
+    * @returns tableau val capteurs de gauche à droite 
+    *  0 blanc (reflexion max)
+    *  1000 noir (pas de reflexion)
+    */
+    void calibrated_sensors(unsigned short ltab[5]);
+ 
     /** Calibrate the sensors. This turns the robot left then right, looking for a line
      *
      */
     char sensor_auto_calibrate (void);
-
+ 
     /** Set calibration manually to the current settings.
      *
      */
     void calibrate(void);
-
+ 
     /** Clear the current calibration settings
      *
      */
     void reset_calibration (void);
-
+ 
     void PID_start(int max_speed, int a, int b, int c, int d);
-
+ 
     void PID_stop();
-
+ 
     /** Write to the 8 LEDs
      *
      * @param leds An 8 bit value to put on the LEDs
      */
     void leds(int val);
-
+ 
     /** Locate the cursor on the 8x2 LCD
      *
      * @param x The horizontal position, from 0 to 7
      * @param y The vertical position, from 0 to 1
      */
     void locate(int x, int y);
-
+ 
     /** Clear the LCD
      *
      */
     void cls(void);
-
+ 
     /** Send a character directly to the 3pi serial interface
      * @param c The character to send to the 3pi
      */
     int putc(int c);
-
+ 
     /** Receive a character directly to the 3pi serial interface
      * @returns c The character received from the 3pi
      */
     int getc();
-
+ 
     /** Send a string buffer to the 3pi serial interface
      * @param text A pointer to a char array
      * @param int The character to send to the 3pi
      */
     int print(char* text, int length);
-
+ 
 #ifdef MBED_RPC
     virtual const struct rpc_method *get_rpc_methods();
 #endif
-
+ 
 private :
-
+ 
     DigitalOut _nrst;
     Serial _ser;
     
     void motor (int motor, float speed);
     virtual int _putc(int c);
     virtual int _getc();
-
+ 
 };
-
+ 
 #endif
\ No newline at end of file