Revision 3:112561b84d80, committed 2019-03-08

Comitter:

equalizer

Date:

Fri Mar 08 15:01:02 2019 +0000

Parent:

2:931ee1f0017e

Commit message:

Mbed Arduino serial com + Bluetooth

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/zumo.cpp	Fri Mar 08 15:01:02 2019 +0000
@@ -0,0 +1,230 @@
+#include "mbed.h"
+#include "zumo.h"
+#include <string>
+
+zumo::zumo(PinName tx, PinName rx) :  Stream("zumo"), _ser(tx, rx)  {
+    _ser.baud(115200);
+}
+
+zumo::zumo() :  Stream("zumo"), _ser(p13, p14)  {
+    _ser.baud(115200);
+}
+
+//test envoi position
+void zumo::get_position(char trame_position[]) {
+    char trame_get_position[24];
+    sprintf(trame_get_position,"SEND_LINE_POSITION,0,0,0,0");
+    _ser.puts(trame_get_position);
+    
+    _ser.scanf(trame_position);
+    }
+
+//test récupération valeur
+void zumo::sensor_get() {
+    /*char trame_capteur[24];
+    _ser.gets(trame_catpeur,24);
+    */
+    }
+
+void zumo::left_motor (float speed) {
+    char trame_motor[24];
+    sprintf(trame_motor,"M,L,0,%.2f,0",speed);
+    _ser.puts(trame_motor);
+}
+
+void zumo::right_motor (float speed) {
+    char trame_motor[24];
+    sprintf(trame_motor,"M,0,R,0,%.2f",speed);
+    _ser.puts(trame_motor);
+}
+
+void zumo::forward (float speed) {
+    char trame_motor[24];
+    sprintf(trame_motor,"M,L,R,%.2f,%.2f",speed,speed);
+    _ser.puts(trame_motor);
+}
+
+void zumo::backward (float speed) {
+    char trame_motor[24];
+    sprintf(trame_motor,"M,L,R,%.2f,%.2f",-speed,-speed);
+    _ser.puts(trame_motor);
+}
+
+void zumo::left (float speed) {
+    char trame_motor[24];
+    sprintf(trame_motor,"M,L,R,%.2f,%.2f",-speed,speed);
+    _ser.puts(trame_motor);
+}
+
+void zumo::right (float speed) {
+    char trame_motor[24];
+    sprintf(trame_motor,"M,L,R,%.2f,%.2f",speed,-speed);
+    _ser.puts(trame_motor);
+}
+
+void zumo::stop (void) {
+    char trame_motor[16];
+    sprintf(trame_motor,"M,L,R,0,0");
+    _ser.puts(trame_motor);
+}
+
+void zumo::motor (int motor, float speed) {
+    char opcode = 0x0;
+    if (speed > 0.0) {
+        if (motor==1)
+            opcode = M1_FORWARD;
+        else
+            opcode = M2_FORWARD;
+    } else {
+        if (motor==1)
+            opcode = M1_BACKWARD;
+        else
+            opcode = M2_BACKWARD;
+    }
+    unsigned char arg = 0x7f * abs(speed);
+
+    _ser.putc(opcode);
+    _ser.putc(arg);
+}
+
+float zumo::battery() {
+    _ser.putc(SEND_BATTERY_MILLIVOLTS);
+    char lowbyte = _ser.getc();
+    char hibyte  = _ser.getc();
+    float v = ((lowbyte + (hibyte << 8))/1000.0);
+    return(v);
+}
+
+
+float zumo::line_position() {
+    int pos = 0;
+    int pos1 = 0;
+    int pos2 = 0;
+    char trame_line_position[24];
+    sprintf(trame_line_position,"SEND_LINE_POSITION,0,0,0,0");
+    _ser.puts(trame_line_position);
+    /*pos1 = _ser.getc();
+    pos2 = _ser.getc();*/
+    pos=_ser.getc();
+    pos += _ser.getc() << 8;
+    
+    float fpos = ((float)pos - 2048.0)/2048.0;
+    //float fpos = (float)pos1 + (float)pos2;
+    return(fpos);  
+}
+
+void zumo::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;
+    }
+}
+
+void zumo::sensor_auto_calibrate() {
+    char trame_auto_calibrate[24];
+    sprintf(trame_auto_calibrate,"AUTO_CALIBRATE");
+    _ser.puts(trame_auto_calibrate);
+}
+
+
+void zumo::calibrate(void) {
+    _ser.putc(PI_CALIBRATE);
+}
+
+void zumo::reset_calibration() {
+    _ser.putc(LINE_SENSORS_RESET_CALIBRATION);
+}
+
+void zumo::PID_start(int max_speed, int a, int b, int c, int d) {
+    _ser.putc(max_speed);
+    _ser.putc(a);
+    _ser.putc(b);
+    _ser.putc(c);
+    _ser.putc(d);
+}
+
+void zumo::PID_stop() {
+    _ser.putc(STOP_PID);
+}
+
+float zumo::pot_voltage(void) {
+    int volt = 0;
+    _ser.putc(SEND_TRIMPOT);
+    volt = _ser.getc();
+    volt += _ser.getc() << 8;
+    return(volt);
+}
+
+
+void zumo::leds(int val) {
+
+    BusOut _leds(p20,p19,p18,p17,p16,p15,p14,p13);
+    _leds = val;
+}
+
+
+void zumo::locate(int x, int y) {
+    _ser.putc(DO_LCD_GOTO_XY);
+    _ser.putc(x);
+    _ser.putc(y);
+}
+
+void zumo::cls(void) {
+    _ser.putc(DO_CLEAR);
+}
+
+int zumo::print (char* text, int length) {
+    _ser.putc(DO_PRINT);  
+    _ser.putc(length);       
+    for (int i = 0 ; i < length ; i++) {
+        _ser.putc(text[i]); 
+    }
+    return(0);
+}
+
+int zumo::_putc (int c) {
+    _ser.putc(DO_PRINT);  
+    _ser.putc(0x1);       
+    _ser.putc(c);         
+    wait (0.001);
+    return(c);
+}
+
+int zumo::_getc (void) {
+    char r = 0;
+    return(r);
+}
+
+int zumo::putc (int c) {
+    return(_ser.putc(c));
+}
+
+int zumo::getc (void) {
+    return(_ser.getc());
+}
+
+
+
+
+
+#ifdef MBED_RPC
+const rpc_method *zumo::get_rpc_methods() {
+    static const rpc_method rpc_methods[] = {{ "forward", rpc_method_caller<zumo, float, &zumo::forward> },
+        { "backward", rpc_method_caller<zumo, float, &zumo::backward> },
+        { "left", rpc_method_caller<zumo, float, &zumo::left> },
+        { "right", rpc_method_caller<zumo, float, &zumo::right> },
+        { "stop", rpc_method_caller<zumo, &zumo::stop> },
+        { "left_motor", rpc_method_caller<zumo, float, &zumo::left_motor> },
+        { "right_motor", rpc_method_caller<zumo, float, &zumo::right_motor> },
+        { "battery", rpc_method_caller<float, zumo, &zumo::battery> },
+        { "line_position", rpc_method_caller<float, zumo, &zumo::line_position> },
+        { "sensor_auto_calibrate", rpc_method_caller<char, zumo, &zumo::sensor_auto_calibrate> },
+
+
+        RPC_METHOD_SUPER(Base)
+    };
+    return rpc_methods;
+}
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/zumo.h	Fri Mar 08 15:01:02 2019 +0000
@@ -0,0 +1,191 @@
+#ifndef ZUMO_H
+#define ZUMO_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
+#define PI_CALIBRATE 0xB4
+#define DO_CLEAR 0xB7
+#define DO_PRINT 0xB8
+#define DO_LCD_GOTO_XY 0xB9
+#define LINE_SENSORS_RESET_CALIBRATION 0xB5
+#define SEND_LINE_POSITION 0xB6
+#define AUTO_CALIBRATE 0xBA
+#define SET_PID 0xBB
+#define STOP_PID 0xBC
+#define M1_FORWARD 0xC1
+#define M1_BACKWARD 0xC2
+#define M2_FORWARD 0xC5
+#define M2_BACKWARD 0xC6
+
+class zumo :  public Stream {
+
+    // Public functions
+public:
+
+    /** Create the zumo object connected to the default pins
+     *
+     * @param tx Serial transmit pin. Default is p13
+     * @param rx Serial receive pin. Default is p14
+     */
+    zumo();
+
+
+    /** Create the zumo object connected to specific pins
+     *
+     */
+    zumo(PinName tx, PinName rx);
+
+
+    /** 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
+     *   0.0 means the line is in the middle
+     *   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
+     *
+     */
+    void 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);
+    
+    
+    //test récupération valeur
+    void sensor_get(void);
+    
+    //test récupération position
+    void get_position(char trame_position[]);
+
+#ifdef MBED_RPC
+    virtual const struct rpc_method *get_rpc_methods();
+#endif
+
+private :
+
+    Serial _ser;
+    
+    void motor (int motor, float speed);
+    virtual int _putc(int c);
+    virtual int _getc();
+
+};
+
+#endif
\ No newline at end of file