Commit 98b730eb authored by Theot Mickael's avatar Theot Mickael
Browse files

Ajout des fonctions de commande des pinces, pompes et verins

parent 5b5fb0cf
/* 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 "SerialHalfDuplex.h"
#include "mbed.h"
#define AX12_WRITE_DEBUG 0
#define AX12_READ_DEBUG 0
#define AX12_TRIGGER_DEBUG 0
#define AX12_DEBUG 0
#define AX12_REG_ID 0x3
#define AX12_REG_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);
/** Set the mode of the servo
* @param mode
* 0 = Positional, default
* 1 = Continuous rotation
*/
int SetMode(int mode);
/** 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);
/** 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);
void setSpeedMove(int goal, int speed);
int _ID;
private :
SerialHalfDuplex _ax12;
int read(int ID, int start, int length, char* data);
int write(int ID, int start, int length, char* data, int flag=0);
};
#endif
/* 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 "SerialHalfDuplex.h"
#include "mbed.h"
#define AX12_WRITE_DEBUG 0
#define AX12_READ_DEBUG 0
#define AX12_TRIGGER_DEBUG 0
#define AX12_DEBUG 0
#define AX12_REG_ID 0x3
#define AX12_REG_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);
/** Set the mode of the servo
* @param mode
* 0 = Positional, default
* 1 = Continuous rotation
*/
int SetMode(int mode);
/** 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);
/** 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);
void setSpeedMove(int goal, int speed);
int _ID;
private :
SerialHalfDuplex _ax12;
int read(int ID, int start, int length, char* data);
int write(int ID, int start, int length, char* data, int flag=0);
};
#endif
......@@ -17,28 +17,58 @@
//==========================================
// PORT SERIAL LIDAR
#define PORT_LIDAR_TX PC_12
#define PORT_LIDAR_RX PD_2
#define PORT_LIDAR_TX PC_12 //NOUVELLE CARTE PC_6
#define PORT_LIDAR_RX PD_2 //NOUVELLE CARTE PC_7
#define PORT_LIDAR_BAUD 115200
#define BUFFER_LIDAR_SIZE 255
//==========================================
// SERVOS AX12 GENERAL
#define SERVO_N_SERIAL_TX PC_6 //PA_0
#define SERVO_N_SERIAL_RX PC_7 //PA_1
#define SERVO_N_SERIAL_TX PC_6 //NOUVELLE CARTE PA_0
#define SERVO_N_SERIAL_RX PC_7 //NOUVELLE CARTE PA_1
#define SERVO_N_NUMBER 3
#define SERVO1_N_ADDR 1
#define SERVO2_N_ADDR 2
#define SERVO3_N_ADDR 3
//==========================================
// TELECOMMANDE DECLENCHEMENT EXPERIENCE
#define EXPERIENCE_ON PA_4
#define EXPERIENCE_OFF PB_0
// TELECOMMANDE DECLENCHEMENT PHARE
#define EXPERIENCE_ON PC_0
#define EXPERIENCE_OFF PC_3
#define EXPERIENCE_SWITCH_DELAY 500ms
#define EXPERIENCE_DELAY 10000ms
//==========================================
// SERVOS PINCES
#define NB_PINCES 5
#define openPWM 0.8 // A CALCULER
#define closePWM 0.1 // A CALCULER
#define PINCE_1 PB_0 //PC_9 // Adresse pince_1 à vérifier selon la version de la carte E/S
#define PINCE_2 PB_6
#define PINCE_3 PA_7
#define PINCE_4 PA_6
#define PINCE_5 PA_5
//==========================================
// SERVOS LINEAIRES (brachés grâce à un cable en Y) + PC_8 (couleur) en secours ?
#define highPosition 1 // A CALCULER
#define lowPosition 0 // A CALCULER
#define VERIN PB_0
//==========================================
// SIGNAUX DIVERS
#define SIGNAL_GO PC_5
//==========================================
// I2C
#define SDA PB_9
#define SCL PB_8
//==========================================
// POMPES ET ELECTROVANNES
#define POMPE_1 PA_10
#define POMPE_2 PB_3
#define ELECTRO_1 PA_8
#define ELECTRO_2 PA_9
#endif
\ No newline at end of file
......@@ -12,8 +12,8 @@ public:
private:
UnbufferedSerial in;
UnbufferedSerial out;
UnbufferedSerial in;
};
#endif
/* mbed R/C Servo Library
* Copyright (c) 2007-2010 sford, cstyles
*
* 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_SERVO_H
#define MBED_SERVO_H
#include "mbed.h"
/** Servo control class, based on a PwmOut
*
* Example:
* @code
* // Continuously sweep the servo through it's full range
* #include "mbed.h"
* #include "Servo.h"
*
* Servo myservo(p21);
*
* int main() {
* while(1) {
* for(int i=0; i<100; i++) {
* myservo = i/100.0;
* wait(0.01);
* }
* for(int i=100; i>0; i--) {
* myservo = i/100.0;
* wait(0.01);
* }
* }
* }
* @endcode
*/
class Servo {
public:
/** Create a servo object connected to the specified PwmOut pin
*
* @param pin PwmOut pin to connect to
*/
Servo(PinName pin);
/** Set the servo position, normalised to it's full range
*
* @param percent A normalised number 0.0-1.0 to represent the full range.
*/
void write(float percent);
/** Read the servo motors current position
*
* @param returns A normalised number 0.0-1.0 representing the full range.
*/
float read();
/** Set the servo position
*
* @param degrees Servo position in degrees
*/
void position(float degrees);
/** Allows calibration of the range and angles for a particular servo
*
* @param range Pulsewidth range from center (1.5ms) to maximum/minimum position in seconds
* @param degrees Angle from centre to maximum/minimum position in degrees
*/
void calibrate(float range = 0.0005, float degrees = 45.0);
/** Shorthand for the write and read functions */
Servo& operator= (float percent);
Servo& operator= (Servo& rhs);
operator float();
protected:
PwmOut _pwm;
float _range;
float _degrees;
float _p;
};
#endif
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:nucleo_f446re]
platform = ststm32
board = nucleo_f446re
framework = mbed
upload_protocol = stlink
build_flags = -Wl,-u_printf_float,-u_scanf_float
;lib_deps =
;armmbed/mbed-drivers @ ^1.5.0
;mbed-chris/AX12@0.0.0+sha.ced71d1b2558
;mbed-mbed-unsupported/SerialHalfDuplex @ 0.0.0-alpha+sha.7802a25daf3b
;armmbed/minar-platform@^1.1.0
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:nucleo_f446re]
platform = ststm32
board = nucleo_f446re
framework = mbed
upload_protocol = stlink
build_flags = -Wl,-u_printf_float,-u_scanf_float
;lib_deps =
;armmbed/mbed-drivers @ ^1.5.0
;mbed-chris/AX12@0.0.0+sha.ced71d1b2558
;mbed-mbed-unsupported/SerialHalfDuplex @ 0.0.0-alpha+sha.7802a25daf3b
;armmbed/minar-platform@^1.1.0
This diff is collapsed.
/* mbed R/C Servo Library
*
* Copyright (c) 2007-2010 sford, cstyles
*
* 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 "Servo.h"
#include "mbed.h"
static float clamp(float value, float min, float max) {
if(value < min) {
return min;
} else if(value > max) {
return max;
} else {
return value;
}
}
Servo::Servo(PinName pin) : _pwm(pin) {
calibrate();
write(0.5);
}
void Servo::write(float percent) {
float offset = _range * 2.0 * (percent - 0.5);
_pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
_p = clamp(percent, 0.0, 1.0);
}
void Servo::position(float degrees) {
float offset = _range * (degrees / _degrees);
_pwm.pulsewidth(0.0015 + clamp(offset, -_range, _range));
}
void Servo::calibrate(float range, float degrees) {
_range = range;
_degrees = degrees;
}
float Servo::read() {
return _p;
}
Servo& Servo::operator= (float percent) {
write(percent);
return *this;
}
Servo& Servo::operator= (Servo& rhs) {
write(rhs.read());
return *this;
}
Servo::operator float() {