TrekkingPhoenix
Refactoring Ironcup 2020
Dependencies: mbed mbed-rtos MotionSensor EthernetInterface
Revision 0:8f5db5085df7, committed 2020-09-21
- Comitter:
- starling
- Date:
- Mon Sep 21 21:42:07 2020 +0000
- Commit message:
- 12 mar 2020
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/PIDController.cpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,111 @@
+#include "PIDController.hpp"
+
+PIDController::PIDController(Motor* motor, PwmOut* servo, FXAS21002* gyro) {
+ this->motor = motor;
+ this->servo = servo;
+ this->gyro = gyro;
+
+ initializeController();
+
+ target_angle = 0;
+ saved_velocity = 0;
+ gyro_reference = 0;
+
+ interrupt_time = 0.02;
+ Ts = interrupt_time*1000;
+}
+
+void PIDController::initializeController() {
+ P = INITIAL_P;
+ I = INITIAL_I;
+ D = INITIAL_D;
+ N = INITIAL_N;
+
+ for (int i = 0; i < 2; i++)
+ {
+ e[i] = 0;
+ ui[i] = 0;
+ ud[i] = 0;
+ }
+}
+
+void PIDController::PIDTrigger() {
+ printf("PIDCntroller \r\n");
+ PIDController::RunPID();
+}
+
+void PIDController::PIDAttach() {
+
+ pid_trigger.attach(std::bind(&PIDController::PIDTrigger, this), 1.0);
+}
+
+void PIDController::PIDDetach() {
+ pid_trigger.detach();
+}
+
+void PIDController::setInterruptTime(float interrupt_time) {
+ this->interrupt_time = interrupt_time;
+}
+
+void PIDController::setPIDConstants(float P, float I, float D, float N) {
+ this->P = P;
+ this->I = I;
+ this->D = D;
+ this->N = N;
+}
+
+void PIDController::setTargetAngle(float target_angle) {
+ this->target_angle = target_angle;
+ if (target_angle > PI)
+ this->target_angle = target_angle - 2 * PI;
+ else if (target_angle < -PI)
+ this->target_angle = target_angle + 2 * PI;
+}
+
+void PIDController::addTargetAngle(float add_angle) {
+ this->target_angle = this->target_angle + add_angle;
+}
+
+void PIDController::Control() {
+ setServoPWM(u*100/(PI/8), *servo);
+ if(abs(e[0]) >= ERROR_THRESH && motor->getVelocity() > MINIMUM_CURVE_VELOCITY) {
+ saved_velocity = motor->getVelocity();
+ motor->setVelocity(MINIMUM_CURVE_VELOCITY);
+ }
+ else if(abs(e[0]) < ERROR_THRESH && motor->getVelocity() != saved_velocity) {
+ motor->setVelocity(saved_velocity);
+ }
+}
+/* PID controller for angular position */
+void PIDController::RunPID() {
+ float feedback = gyro->get_angle() - gyro_reference;
+ e[0]= e[0] - 2*PI;
+ if(e[0] < -PI)
+ e[0] = e[0] + 2*PI;
+
+ /* Proportinal Part */
+ up[0] = e[0]*P;
+
+ /* Integral Part */
+ ui[1] = ui[0];
+ if(abs(u) < PI/8){
+ ui[0] = (P*I*Ts)*e[1] + ui[1];
+ }
+ else if(u > 0)
+ ui[0] = PI/8 - up[0];
+ else if(u < 0)
+ ui[0] = -PI/8 - up[0];
+
+ /* Derivative Part */
+ ud[1] = ud[0];
+ ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1);
+
+ /** Sum of all parts **/
+ u = up[0] + ud[0] + ui[0];
+
+ Control();
+}
+
+void PIDController::setGyroReference(float gyro_ref) {
+ this->gyro_reference = gyro_ref;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Control/PIDController.hpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,74 @@
+#ifndef __PIDCONTROLLER__
+#define __PIDCONTROLLER__
+
+#include "mbed.h"
+#include "Motor.hpp"
+#include "FXAS21002.h"
+#include "FXOS8700Q.h"
+#include "rtos.h"
+
+#define PI 3.141593
+
+#define INITIAL_P 0.452531214933414
+#define INITIAL_I 5.45748932024049
+#define INITIAL_D 0.000233453623255507
+#define INITIAL_N 51.0605584484153
+
+#define ERROR_THRESH PI/5
+#define MINIMUM_CURVE_VELOCITY 19
+
+class PIDController
+{
+ private:
+
+ PwmOut* servo;
+ Motor* motor;
+ FXAS21002* gyro;
+
+ float target_angle;
+ float P;
+ float I;
+ float D;
+ float N;
+ float e[2];
+ float up[2];
+ float ui[2];
+ float ud[2];
+ float u;
+ float saved_velocity;
+ float gyro_reference;
+
+ Ticker pid_trigger;
+ float interrupt_time;
+ float Ts;
+
+ void PIDTrigger();
+
+ void Control();
+
+ void RunPID();
+
+ public:
+ PIDController(Motor* motor, PwmOut* servo, FXAS21002* gyro);
+
+ void initializeController();
+ // Atach PIDController.
+ void PIDAttach();
+
+ // Detach PIDController.
+ void PIDDetach();
+
+ void setInterruptTime(float interrupt_time);
+
+ void setPIDConstants(float P, float I, float D, float N);
+
+ void setTargetAngle(float target_angle);
+
+ void addTargetAngle(float add_angle);
+
+ void setGyroReference(float gyro_ref);
+
+
+};
+
+#endif //__PIDCONTROLLER__
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/EthernetInterface.lib Mon Sep 21 21:42:07 2020 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/mbed_official/code/EthernetInterface/#4d7bff17a592
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LED/LED.cpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,100 @@
+#include "LED.hpp"
+
+LED::LED(PinName red_main, PinName green_main, PinName blue_main,
+ PinName red_nxp, PinName green_nxp, PinName blue_nxp):
+ red_led_main(red_main),
+ green_led_main(green_main),
+ blue_led_main(blue_main),
+ red_led_nxp(red_nxp),
+ green_led_nxp(green_nxp),
+ blue_led_nxp(blue_nxp)
+ {
+ RGB_LED_ON = 0; //active Low
+ RGB_LED_OFF = 1; //active Low
+ MAIN_RGB_LED_ON = 1; //active HIGH
+ MAIN_RGB_LED_OFF = 0;
+ }
+
+void LED::turn_leds_off_nxp() {
+ red_led_nxp = RGB_LED_OFF;
+ green_led_nxp = RGB_LED_OFF;
+ blue_led_nxp = RGB_LED_OFF;
+}
+
+void LED::set_leds_color_nxp(int color) {
+ turn_leds_off_nxp();
+
+ switch(color) {
+ case RED:
+ turn_leds_off_nxp();
+ red_led_nxp = RGB_LED_ON;
+ break;
+ case GREEN:
+ turn_leds_off_nxp();
+ green_led_nxp = RGB_LED_ON;
+ break;
+ case BLUE:
+ turn_leds_off_nxp();
+ blue_led_nxp = RGB_LED_ON;
+ break;
+ case BLACK:
+ turn_leds_off_nxp();
+ break;
+ case WHITE:
+ red_led_nxp = RGB_LED_ON;
+ green_led_nxp = RGB_LED_ON;
+ blue_led_nxp = RGB_LED_ON;
+ break;
+ case PURPLE:
+ turn_leds_off_nxp();
+ red_led_nxp = RGB_LED_ON;
+ blue_led_nxp = RGB_LED_ON;
+ break;
+ case YELLOW:
+ turn_leds_off_nxp();
+ red_led_nxp = RGB_LED_ON;
+ green_led_nxp = RGB_LED_ON;
+ break;
+ case AQUA:
+ turn_leds_off_nxp();
+ blue_led_nxp = RGB_LED_ON;
+ green_led_nxp = RGB_LED_ON;
+ break;
+ default:
+ break;
+ }
+
+}
+
+void LED::turn_leds_off_main() {
+ red_led_main = MAIN_RGB_LED_OFF;
+ green_led_main = MAIN_RGB_LED_OFF;
+ blue_led_main = MAIN_RGB_LED_OFF;
+}
+
+void LED::set_leds_color_main(int color) {
+ switch(color) {
+ case RED:
+ turn_leds_off_main();
+ red_led_main = MAIN_RGB_LED_ON;
+ break;
+ case GREEN:
+ turn_leds_off_main();
+ green_led_main = MAIN_RGB_LED_ON;
+ break;
+ case BLUE:
+ turn_leds_off_main();
+ blue_led_main = MAIN_RGB_LED_ON;
+ break;
+ case BLACK:
+ turn_leds_off_main();
+ break;
+ case WHITE:
+ red_led_main = MAIN_RGB_LED_ON;
+ green_led_main = MAIN_RGB_LED_ON;
+ blue_led_main = MAIN_RGB_LED_ON;
+ break;
+ default:
+ break;
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/LED/LED.hpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,46 @@
+#ifndef __LED__
+#define __LED__
+
+#include "mbed.h"
+
+
+class LED {
+ private:
+ int RGB_LED_ON; //active Low
+ int RGB_LED_OFF; //active Low
+ int MAIN_RGB_LED_ON; //active HIGH
+ int MAIN_RGB_LED_OFF;
+
+ // Leds Objects
+ DigitalOut red_led_main;
+ DigitalOut green_led_main;
+ DigitalOut blue_led_main;
+
+ DigitalOut red_led_nxp;
+ DigitalOut green_led_nxp;
+ DigitalOut blue_led_nxp;
+
+
+ public:
+ enum{
+ BLACK,
+ RED,
+ GREEN,
+ BLUE,
+ WHITE,
+ PURPLE,
+ YELLOW,
+ AQUA};
+
+ LED(PinName red_main, PinName green_main, PinName blue_main,
+ PinName red_nxp, PinName green_nxp, PinName blue_nxp);
+
+ void set_leds_color_nxp(int color);
+ void turn_leds_off_nxp();
+
+ void set_leds_color_main(int color);
+ void turn_leds_off_main();
+
+
+};
+#endif //__LED__
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MotionSensor.lib Mon Sep 21 21:42:07 2020 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/teams/components/code/MotionSensor/#4d6e28d4a18a
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.cpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,88 @@
+
+#include "Motor.hpp"
+
+#define PI 3.141592653589793238462
+#define PWM_PERIOD 13.5 // ms
+#define BRAKE_CONSTANT 100 // Brake force(max = 100)
+#define BRAKE_WAIT 1.662 // seconds
+#define MINIMUM_VELOCITY 15
+
+Motor::Motor(PinName motor_pin): motor(motor_pin){}
+
+
+void Motor::startJogging(float jog_dc, float jog_p){
+ jog_duty_cycle = jog_dc;
+ jog_period = jog_p;
+ interruption.attach(this,&Motor::motorJogging, jog_duty_cycle*jog_period);
+}
+
+void Motor::stopJogging(void){
+ interruption.detach();
+ setMotorPWM(velocity,motor);
+}
+
+void Motor::motorJogging(void) {
+ interruption.detach();
+ if(!alternate_motor){
+ setMotorPWM(velocity, motor);
+ interruption.attach(this,&Motor::motorJogging, jog_duty_cycle*jog_period);
+ }
+ else{
+ setMotorPWM(10, motor);
+ interruption.attach(this,&Motor::motorJogging, (1-jog_duty_cycle)*jog_period);
+ }
+ alternate_motor = !alternate_motor;
+}
+
+void Motor::brakeMotor(float brake_intensity, float brake_wait){
+ stopJogging();
+ if(velocity >= 0){
+ setMotorPWM(-brake_intensity, motor);
+ wait(brake_wait);
+ velocity = 0;
+ setMotorPWM(velocity,motor);
+ }
+ else {
+ setVelocity(0);
+ }
+}
+
+void Motor::reverseMotor(int speed){
+ for(int i=0 ; i >= -speed; i--){
+ setMotorPWM((float)i,motor);
+ wait_ms(13.5);
+ }
+ for(int i=-speed ; i <= 0; i++){
+ setMotorPWM((float)i,motor);
+ wait_ms(13.5);
+ }
+ for(int i=0 ; i >= -speed; i--){
+ setMotorPWM((float)i,motor);
+ wait_ms(13.5);
+ }
+}
+
+void Motor::setSmoothVelocity(int new_velocity){
+ if( velocity > new_velocity){
+ for(; velocity >= new_velocity; velocity--){
+ setMotorPWM(velocity,motor);
+ wait_ms(PWM_PERIOD);
+ }
+ velocity++;
+ }
+ else if(velocity < new_velocity){
+ for(; velocity <= new_velocity; velocity++){
+ setMotorPWM(velocity,motor);
+ wait_ms(PWM_PERIOD);
+ }
+ velocity--;
+ }
+}
+
+void Motor::setVelocity(int new_velocity){
+ setMotorPWM(new_velocity,motor);
+ velocity = new_velocity;
+}
+float Motor::getVelocity(){
+ return velocity;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor/Motor.hpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,32 @@
+#ifndef MOTOR_HEADER
+#define MOTOR_HEADER
+
+#include "mbed.h"
+#include "CarPWM.h"
+
+class Motor{
+
+ protected:
+ Ticker interruption;
+ bool alternate_motor;
+ float velocity;
+ float jog_duty_cycle;
+ float jog_period;
+ PwmOut motor;
+
+ public:
+
+ void startJogging(float jog_dc, float jog_p);
+ void stopJogging(void);
+ void brakeMotor(float brake_intensity, float brake_wait);
+ void reverseMotor(int speed);
+ void setVelocity(int new_velocity);
+ float getVelocity();
+ void setSmoothVelocity(int new_velocity);
+ Motor(PinName motor_pin);
+
+ private:
+ void motorJogging(void);
+
+};
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/CarPWM.cpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,38 @@
+#include "CarPWM.h"
+#include "mbed.h"
+
+#define MINIMUM_MOTOR_WIDTH 1160 // microseconds
+#define ZERO_MOTOR_WIDTH 1505 // microseconds
+#define MAXIMUM_MOTOR_WIDTH 1910 // microseconds
+#define MINIMUM_SERVO_WIDTH 1016 // microseconds
+#define ZERO_SERVO_WIDTH 1470 // microseconds
+#define MAXIMUM_SERVO_WIDTH 1858 // microseconds
+#define PERIOD 13500 // miliseconds
+#define MOTOR_RANGE_MAX 100
+#define SERVO_ANGLE_MAX 100
+
+void initialize(PwmOut motor, PwmOut servo){
+ motor.period_us(PERIOD);
+ servo.period_us(PERIOD);
+ }
+void setServoPWM(float angle, PwmOut servo){
+ if(angle > SERVO_ANGLE_MAX)
+ angle = SERVO_ANGLE_MAX;
+ else if(angle < -SERVO_ANGLE_MAX)
+ angle = - SERVO_ANGLE_MAX;
+ float servo_width = ZERO_SERVO_WIDTH + (angle/SERVO_ANGLE_MAX)*(MAXIMUM_SERVO_WIDTH - ZERO_SERVO_WIDTH);
+ servo.pulsewidth_us(servo_width);
+ // Serial pc(USBTX, USBRX);
+ // pc.printf("\nServo: %f ",servo_width);
+}
+void setMotorPWM(float speed, PwmOut motor){
+ if(speed > MOTOR_RANGE_MAX)
+ speed = MOTOR_RANGE_MAX;
+ else if(speed < -MOTOR_RANGE_MAX)
+ speed = - MOTOR_RANGE_MAX;
+ float motor_speed = ZERO_MOTOR_WIDTH + (speed/MOTOR_RANGE_MAX)*(MAXIMUM_MOTOR_WIDTH - ZERO_MOTOR_WIDTH);
+ motor.pulsewidth_us(motor_speed);
+ // Serial pc(USBTX, USBRX);
+ // pc.printf("Motor: %f ",motor_speed);
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PWM/CarPWM.h Mon Sep 21 21:42:07 2020 +0000 @@ -0,0 +1,10 @@ +#include "mbed.h" +#ifndef CARPWM_H +#define CARPWM_H + +void initialize(PwmOut motor, PwmOut servo); +void setServoPWM(float angle, PwmOut servo); +void setMotorPWM(float width, PwmOut motor); + + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/Protocol/protocol.h Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,154 @@
+/**
+@file protocol.h
+@brief Protocol definitions.
+*/
+
+/*
+Copyright 2016 Erik Perillo <erik.perillo@gmail.com>
+
+This file is part of piranha-ptc.
+
+This is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef __PIRANHA_PROTOCOL_H__
+#define __PIRANHA_PROTOCOL_H__
+
+//@{
+///PID parameters range.
+#define PID_PARAMS_MIN 0.0
+#define PID_PARAMS_MAX 100.0
+//@}
+
+//@{
+///Ground velocity range.
+#define GND_VEL_MIN -100.0
+#define GND_VEL_MAX 100.0
+//@}
+
+//@{
+///Angle reference range (in radians).
+#define PI 3.141593
+#define ABS_ANG_REF_MIN -PI
+#define ABS_ANG_REF_MAX PI
+//@}
+
+//@{
+///Angle reference from robot range (in radians).
+#define REL_ANG_REF_MIN -PI
+#define REL_ANG_REF_MAX PI
+//@}
+
+//@{
+///Angle reference from magnetometer range (in radians).
+#define MAG_ANG_REF_MIN -PI
+#define MAG_ANG_REF_MAX PI
+//@}
+
+//@{
+///Break intensity
+#define BRAKE_INTENSITY_MIN 0.0
+#define BRAKE_INTENSITY_MAX 100.0
+//@}
+
+//@{
+///Jogging speed ratio.
+#define BRAKE_PERIOD_MIN 0.0
+#define BRAKE_PERIOD_MAX 100.0
+//@}
+
+//@{
+///Jogging speed period (in seconds).
+#define JOG_VEL_PERIOD_MIN 0.0
+#define JOG_VEL_PERIOD_MAX 300.0
+//@}
+
+//@{
+///Jogging speed ratio.
+#define JOG_VEL_RATIO_MIN 0.0
+#define JOG_VEL_RATIO_MAX 1.0
+//@}
+
+//@{
+///Magnetometer calibration values.
+#define MAG_CALIB_MIN -750.0
+#define MAG_CALIB_MAX 750.0
+//@}
+
+///Messages to send via protocol.
+enum
+{
+ ///Do nothing.
+ NONE,
+
+ ///Brake the robot.
+ BRAKE,
+
+ ///Reset gyroscope.
+ GYRO_ZERO,
+
+ ///Set zero axis to current angle measure.
+ ANG_SET,
+
+ ///Reset angle zero axis.
+ ANG_RST,
+
+ ///Turn on leds
+ LED_ON,
+
+ ///Turn off leds
+ LED_OFF,
+
+ ///Set new angle reference relative to zero axis.
+ ABS_ANG_REF,
+
+ ///Set new angle reference relative to robot axis.
+ REL_ANG_REF,
+
+ ///Set new angle reference relative to north using magnetometer.
+ MAG_ANG_REF,
+
+ ///Set new ground velocity for robot.
+ GND_VEL,
+
+ ///Set new jogging speed for robot.
+ JOG_VEL,
+
+ ///Magnetometer calibration (min_x, max_x, min_y, max_y).
+ MAG_CALIB,
+
+ ///Send PID control parameters (P, I, D, N).
+ PID_PARAMS
+};
+
+#define MSG_HEADER_SIZE 1
+#define MSG_VAL_SIZE 2
+#define MSG_MAX_NUM_VALS 4
+#define MSG_BUF_LEN (MSG_HEADER_SIZE + MSG_VAL_SIZE*MSG_MAX_NUM_VALS)
+#define MSG_HEADER_IDX 0
+#define MSG_VALS_START_IDX (MSG_HEADER_IDX + 1)
+
+#define SENDER_PORT 7532
+#define SENDER_IFACE_ADDR "192.168.7.2"
+#define SENDER_NETMASK_ADDR "255.255.255.0"
+#define SENDER_GATEWAY_ADDR "0.0.0.0"
+
+#define RECEIVER_PORT 7533
+#define RECEIVER_IFACE_ADDR "192.168.7.3"
+#define RECEIVER_NETMASK_ADDR "255.255.255.0"
+#define RECEIVER_GATEWAY_ADDR "0.0.0.0"
+
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/Protocol/receiver.cpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,159 @@
+/*
+Copyright 2016 Erik Perillo <erik.perillo@gmail.com>
+
+This file is part of piranha-ptc.
+
+This is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "receiver.h"
+#include "EthernetInterface.h"
+
+float Receiver::un_scale(uint16_t value, float min, float max)
+{
+ return ((float)value)/((1 << 16) - 1)*(max - min) + min;
+}
+
+uint8_t Receiver::get_header()
+{
+ return this->message[MSG_HEADER_IDX];
+}
+
+uint16_t Receiver::get_raw_val(int pos)
+{
+ uint16_t value = 0;
+
+ value |= this->message[MSG_VALS_START_IDX + 2*pos];
+ value |= this->message[MSG_VALS_START_IDX + 2*pos + 1] << 8;
+
+ return value;
+}
+
+float Receiver::get_val(float min, float max, int pos)
+{
+ uint16_t raw_val;
+
+ raw_val = this->get_raw_val(pos);
+ return this->un_scale(raw_val, min, max);
+}
+
+void Receiver::get_vals(float min, float max, float* vals, int size)
+{
+ uint16_t raw_val;
+
+ for(int i=0; i<size; i++)
+ {
+ raw_val = this->get_raw_val(i);
+ vals[i] = this->un_scale(raw_val, min, max);
+ }
+}
+
+bool Receiver::receive()
+{
+ return this->sock.receiveFrom(this->sender_addr, this->message,
+ sizeof(this->message)) > 0;
+}
+
+Receiver::Receiver()
+{
+ ;
+}
+
+Receiver::Receiver(Endpoint sender_addr, const UDPSocket& sock):
+ sock(sock), sender_addr(sender_addr)
+{
+ ;
+}
+
+Receiver::Receiver(Endpoint sender_addr, int sock_port, int timeout):
+ sender_addr(sender_addr)
+{
+ this->sock.bind(sock_port);
+ this->sock.set_blocking(timeout < 0, timeout);
+}
+
+void Receiver::set_sender_addr(const Endpoint& sender_addr)
+{
+ this->sender_addr = sender_addr;
+}
+
+void Receiver::set_socket(const UDPSocket& sock)
+{
+ this->sock = sock;
+}
+
+void Receiver::set_socket(int port, int timeout)
+{
+ this->sock.bind(port);
+ this->sock.set_blocking(timeout < 0, timeout);
+}
+
+Endpoint Receiver::get_sender_addr()
+{
+ return this->sender_addr;
+}
+
+UDPSocket Receiver::get_socket()
+{
+ return this->sock;
+}
+
+uint8_t Receiver::get_msg()
+{
+ return this->message[MSG_HEADER_IDX];
+}
+
+float Receiver::get_abs_ang_ref()
+{
+ return this->get_val(ABS_ANG_REF_MIN, ABS_ANG_REF_MAX);
+}
+
+float Receiver::get_rel_ang_ref()
+{
+ return this->get_val(REL_ANG_REF_MIN, REL_ANG_REF_MAX);
+}
+
+float Receiver::get_mag_ang_ref()
+{
+ return this->get_val(MAG_ANG_REF_MIN, MAG_ANG_REF_MAX);
+}
+
+float Receiver::get_gnd_vel()
+{
+ return this->get_val(GND_VEL_MIN, GND_VEL_MAX);
+}
+
+void Receiver::get_brake(float* intensity, float* period)
+{
+ *intensity = this->get_val(BRAKE_INTENSITY_MIN, BRAKE_INTENSITY_MAX);
+ *period = this->get_val(BRAKE_PERIOD_MIN, BRAKE_PERIOD_MAX, 1);
+}
+
+void Receiver::get_jog_vel(float* period, float* ratio)
+{
+ *period = this->get_val(JOG_VEL_PERIOD_MIN, JOG_VEL_PERIOD_MAX);
+ *ratio = this->get_val(JOG_VEL_RATIO_MIN, JOG_VEL_RATIO_MAX, 1);
+}
+
+void Receiver::get_pid_params(float* params)
+{
+ this->get_vals(PID_PARAMS_MIN, PID_PARAMS_MAX, params, 4);
+}
+
+void Receiver::get_mag_calib(float* params)
+{
+ this->get_vals(MAG_CALIB_MIN, MAG_CALIB_MAX, params, 4);
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Protocol/Protocol/receiver.h Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,77 @@
+/**
+@file receiver.h
+@brief Receiver side functions declarations.
+*/
+
+/*
+Copyright 2016 Erik Perillo <erik.perillo@gmail.com>
+
+This file is part of piranha-ptc.
+
+This is free software: you can redistribute it and/or modify
+it under the terms of the GNU General Public License as published by
+the Free Software Foundation, either version 3 of the License, or
+(at your option) any later version.
+
+This is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+See the GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef __PIRANHA_RCV_PROTOCOL_H__
+#define __PIRANHA_RCV_PROTOCOL_H__
+
+#include "protocol.h"
+#include "mbed.h"
+#include "EthernetInterface.h"
+
+#define TEST
+
+class Receiver
+{
+
+ #ifdef TEST
+ public:
+ #else
+ protected:
+ #endif
+ UDPSocket sock;
+ char message[MSG_BUF_LEN];
+ Endpoint sender_addr;
+
+ float un_scale(uint16_t value, float min, float max);
+ uint8_t get_header();
+ uint16_t get_raw_val(int pos=0);
+ float get_val(float min, float max, int pos=0);
+ void get_vals(float min, float max, float* vals, int size);
+
+ public:
+ Receiver();
+ Receiver(Endpoint sender_addr, const UDPSocket& sock);
+ Receiver(Endpoint sender_addr, int sock_port=RECEIVER_PORT, int timeout=1);
+
+ void set_sender_addr(const Endpoint& sender_addr);
+ void set_socket(const UDPSocket& sock);
+ void set_socket(int port=RECEIVER_PORT, int timeout=1);
+ Endpoint get_sender_addr();
+ UDPSocket get_socket();
+
+ bool receive();
+ uint8_t get_msg();
+ float get_abs_ang_ref();
+ float get_rel_ang_ref();
+ float get_mag_ang_ref();
+ float get_gnd_vel();
+ void get_brake(float* intensity, float* period);
+ void get_jog_vel(float* period, float* ratio);
+ void get_pid_params(float* params);
+ void get_mag_calib(float* params);
+};
+
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXAS21002.cpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,106 @@
+ /* Copyright (c) 2015 NXP Semiconductors. MIT License
+*
+* 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 "FXAS21002.h"
+#include "mbed.h"
+FXAS21002::FXAS21002(PinName sda, PinName scl) : gyroi2c(sda,scl)
+{
+
+}
+
+void FXAS21002::set_gyro(gyro_mode mode) // Protected method
+{
+ char d[2];
+ d[0] = FXAS21002_CTRL_REG1; //Puts device in standby mode
+ d[1] = 0x08;
+ gyroi2c.write(FXAS21002_I2C_ADDRESS, d,2);
+
+ d[0] = FXAS21002_CTRL_REG0; //Sets FSR and Sensitivity
+ d[1] = mode + 0x80;
+ gyroi2c.write(FXAS21002_I2C_ADDRESS, d, 2);
+
+ d[0] = FXAS21002_CTRL_REG1; //Puts device in active mode
+ d[1] = 0x0A;
+ gyroi2c.write(FXAS21002_I2C_ADDRESS, d,2);
+}
+
+
+void FXAS21002::stop_measure(void)
+{
+ interrupt.detach();
+ angle = 0;
+}
+
+float FXAS21002::get_angle(void)
+{
+ return angle;
+}
+
+void FXAS21002::integrate_gyro_angle(void)
+{
+ float gyro_data[3];
+ acquire_gyro_data_dps(gyro_data);
+
+ angle = angle + (gyro_data[2]-GYRO_OFFSET)*(period/1000000);
+ if(angle > 180)
+ angle = angle - 360;
+ if(angle < -180)
+ angle = angle + 360;
+}
+
+void FXAS21002::start_measure(float period_us)
+{
+ period = period_us;
+ interrupt.attach_us(this,&FXAS21002::integrate_gyro_angle,period);
+ angle = 0;
+}
+
+void FXAS21002::gyro_config(void)
+{
+ set_gyro(MODE_2); //Default implementation
+ sensitivity = 0.03125;
+}
+
+void FXAS21002::gyro_config(gyro_mode mode)
+{
+ set_gyro(mode);
+ switch(mode)
+ {
+ case MODE_1: sensitivity = 0.0625; break;
+ case MODE_2: sensitivity = 0.03125; break;
+ case MODE_3: sensitivity = 0.015625; break;
+ case MODE_4: sensitivity = 0.0078125; break;
+ }
+}
+
+void FXAS21002::acquire_gyro_data_dps(float * g_data)
+{
+
+ char data_bytes[7];
+ gyroi2c.write(FXAS21002_I2C_ADDRESS,FXAS21002_STATUS,1,true); // Read the 6 data bytes - LSB and MSB for X, Y and Z Axes.
+ gyroi2c.read(FXAS21002_I2C_ADDRESS,data_bytes,7);
+
+ g_data[0] = (float)((int16_t)((data_bytes[1]*256) + (data_bytes[2]))) * sensitivity;
+ g_data[1] = (float)((int16_t)((data_bytes[3]*256) + (data_bytes[4]))) * sensitivity;
+ g_data[2] = (float)((int16_t)((data_bytes[5]*256) + (data_bytes[6]))) * sensitivity;
+
+}
+
+
+
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXAS21002.h Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,71 @@
+ /* Copyright (c) 2015 NXP Semiconductors. MIT License
+*
+* 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 FXAS21002_H
+#define FXAS21002_H
+#include "mbed.h"
+
+#define FXAS21002_I2C_ADDRESS 0x40
+
+#define FXAS21002_STATUS 0x00
+#define FXAS21002_WHO_AM_I 0x0C
+#define FXAS21002_CTRL_REG0 0x0D
+#define FXAS21002_CTRL_REG1 0x13
+#define FXAS21002_WHO_AM_I_VALUE 0xD1
+#define GYRO_OFFSET -0.239 //-0.15 //-0.2396875
+/* Gyroscope mechanical modes
+* Mode Full-scale range [Deg/s] Sensitivity [(mDeg/s)/LSB]
+* 1 +- 2000 62.5
+* 2 +- 1000 31.25
+* 3 +- 500 15.625
+* 4 +- 250 7.8125
+*/
+enum gyro_mode
+{
+ MODE_1 = 0x00,
+ MODE_2 = 0x01,
+ MODE_3 = 0x02,
+ MODE_4 = 0x03
+};
+
+class FXAS21002
+{
+ public:
+
+ FXAS21002(PinName sda, PinName scl);
+
+ void gyro_config(void);
+ void gyro_config(gyro_mode mode);
+
+ void acquire_gyro_data_dps(float * du);
+ void start_measure(float period_us);
+ void stop_measure(void);
+ float get_angle(void);
+
+ private:
+ void set_gyro(gyro_mode mode);
+ void integrate_gyro_angle(void);
+ Ticker interrupt;
+ I2C gyroi2c;
+ float sensitivity;
+ float angle;
+ float period;
+
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXOS8700Q.cpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,294 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* 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 "FXOS8700Q.h"
+#define UINT14_MAX 16383
+
+
+FXOS8700Q_acc::FXOS8700Q_acc(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
+ // activate the peripheral
+ uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
+ m_i2c.frequency(400000);
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_M_CTRL_REG1;
+ data[1] = 0x1F;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_M_CTRL_REG2;
+ data[1] = 0x20;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_XYZ_DATA_CFG;
+ data[1] = 0x00;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_CTRL_REG1;
+ data[1] = 0x18;//0x1D;
+ writeRegs(data, 2);
+}
+
+FXOS8700Q_acc::~FXOS8700Q_acc() { }
+
+void FXOS8700Q_acc::enable(void) {
+ uint8_t data[2];
+ readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+ data[1] |= 0x01;
+ data[0] = FXOS8700Q_CTRL_REG1;
+ writeRegs(data, 2);
+}
+
+void FXOS8700Q_acc::disable(void) {
+ uint8_t data[2];
+ readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+ data[1] &= 0xFE;
+ data[0] = FXOS8700Q_CTRL_REG1;
+ writeRegs(data, 2);
+}
+
+
+
+uint32_t FXOS8700Q_acc::whoAmI() {
+ uint8_t who_am_i = 0;
+ readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
+ return (uint32_t) who_am_i;
+}
+
+uint32_t FXOS8700Q_acc::dataReady(void) {
+ uint8_t stat = 0;
+ readRegs(FXOS8700Q_STATUS, &stat, 1);
+ return (uint32_t) stat;
+}
+
+uint32_t FXOS8700Q_acc::sampleRate(uint32_t f) {
+ return(50); // for now sample rate is fixed at 50Hz
+}
+
+void FXOS8700Q_acc::getX(float * x) {
+ *x = (float(getAccAxis(FXOS8700Q_OUT_X_MSB))/4096.0f);
+}
+
+void FXOS8700Q_acc::getY(float * y) {
+ *y = (float(getAccAxis(FXOS8700Q_OUT_Y_MSB))/4096.0f);
+}
+
+void FXOS8700Q_acc::getZ(float * z) {
+ *z = (float(getAccAxis(FXOS8700Q_OUT_Z_MSB))/4096.0f);
+}
+
+void FXOS8700Q_acc::getX(int16_t * d) {
+ *d = getAccAxis(FXOS8700Q_OUT_X_MSB);
+}
+
+void FXOS8700Q_acc::getY(int16_t * d) {
+ *d = getAccAxis(FXOS8700Q_OUT_Y_MSB);
+}
+
+void FXOS8700Q_acc::getZ(int16_t * d) {
+ *d = getAccAxis(FXOS8700Q_OUT_Z_MSB);
+}
+
+
+void FXOS8700Q_acc::getAxis(MotionSensorDataUnits &data) {
+ int16_t acc, t[3];
+ uint8_t res[6];
+ readRegs(FXOS8700Q_OUT_X_MSB, res, 6);
+
+ acc = (res[0] << 6) | (res[1] >> 2);
+ if (acc > UINT14_MAX/2)
+ acc -= UINT14_MAX;
+ t[0] = acc;
+ acc = (res[2] << 6) | (res[3] >> 2);
+ if (acc > UINT14_MAX/2)
+ acc -= UINT14_MAX;
+ t[1] = acc;
+ acc = (res[4] << 6) | (res[5] >> 2);
+ if (acc > UINT14_MAX/2)
+ acc -= UINT14_MAX;
+ t[2] = acc;
+ data.x = ((float) t[0]) / 4096.0f;
+ data.y = ((float) t[1]) / 4096.0f;
+ data.z = ((float) t[2]) / 4096.0f;
+}
+
+
+void FXOS8700Q_acc::getAxis(MotionSensorDataCounts &data) {
+ int16_t acc;
+ uint8_t res[6];
+ readRegs(FXOS8700Q_OUT_X_MSB, res, 6);
+
+ acc = (res[0] << 6) | (res[1] >> 2);
+ if (acc > UINT14_MAX/2)
+ acc -= UINT14_MAX;
+ data.x = acc;
+ acc = (res[2] << 6) | (res[3] >> 2);
+ if (acc > UINT14_MAX/2)
+ acc -= UINT14_MAX;
+ data.y = acc;
+ acc = (res[4] << 6) | (res[5] >> 2);
+ if (acc > UINT14_MAX/2)
+ acc -= UINT14_MAX;
+ data.z = acc;
+}
+
+void FXOS8700Q_acc::readRegs(int addr, uint8_t * data, int len) {
+ char t[1] = {addr};
+ m_i2c.write(m_addr, t, 1, true);
+ m_i2c.read(m_addr, (char *)data, len);
+}
+
+void FXOS8700Q_acc::writeRegs(uint8_t * data, int len) {
+ m_i2c.write(m_addr, (char *)data, len);
+}
+
+
+int16_t FXOS8700Q_acc::getAccAxis(uint8_t addr) {
+ int16_t acc;
+ uint8_t res[2];
+ readRegs(addr, res, 2);
+
+ acc = (res[0] << 6) | (res[1] >> 2);
+ if (acc > UINT14_MAX/2)
+ acc -= UINT14_MAX;
+
+ return acc;
+}
+
+
+
+FXOS8700Q_mag::FXOS8700Q_mag(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
+ // activate the peripheral
+ uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
+ m_i2c.frequency(400000);
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_M_CTRL_REG1;
+ data[1] = 0x1F;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_M_CTRL_REG2;
+ data[1] = 0x20;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_XYZ_DATA_CFG;
+ data[1] = 0x00;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_CTRL_REG1;
+ data[1] = 0x18;//0x1D;
+ writeRegs(data, 2);
+}
+
+FXOS8700Q_mag::~FXOS8700Q_mag() { }
+
+void FXOS8700Q_mag::enable(void) {
+ uint8_t data[2];
+ readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+ data[1] |= 0x01;
+ data[0] = FXOS8700Q_CTRL_REG1;
+ writeRegs(data, 2);
+}
+
+void FXOS8700Q_mag::disable(void) {
+ uint8_t data[2];
+ readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+ data[1] &= 0xFE;
+ data[0] = FXOS8700Q_CTRL_REG1;
+ writeRegs(data, 2);
+}
+
+
+
+uint32_t FXOS8700Q_mag::whoAmI() {
+ uint8_t who_am_i = 0;
+ readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
+ return (uint32_t) who_am_i;
+}
+
+uint32_t FXOS8700Q_mag::dataReady(void) {
+ uint8_t stat = 0;
+ readRegs(FXOS8700Q_STATUS, &stat, 1);
+ return (uint32_t) stat;
+}
+
+uint32_t FXOS8700Q_mag::sampleRate(uint32_t f) {
+ return(50); // for now sample rate is fixed at 50Hz
+}
+
+void FXOS8700Q_mag::getX(float * x) {
+ *x = (float(getAccAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f);
+}
+
+void FXOS8700Q_mag::getY(float * y) {
+ *y = (float(getAccAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f);
+}
+
+void FXOS8700Q_mag::getZ(float * z) {
+ *z = (float(getAccAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f);
+}
+
+void FXOS8700Q_mag::getX(int16_t * d) {
+ *d = getAccAxis(FXOS8700Q_M_OUT_X_MSB);
+}
+
+void FXOS8700Q_mag::getY(int16_t * d) {
+ *d = getAccAxis(FXOS8700Q_M_OUT_Y_MSB);
+}
+
+void FXOS8700Q_mag::getZ(int16_t * d) {
+ *d = getAccAxis(FXOS8700Q_M_OUT_Z_MSB);
+}
+
+
+void FXOS8700Q_mag::getAxis(MotionSensorDataUnits &data) {
+ int16_t t[3];
+ uint8_t res[6];
+ readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
+
+ t[0] = (res[0] << 8) | res[1];
+ t[1] = (res[2] << 8) | res[3];
+ t[2] = (res[4] << 8) | res[5];
+
+ data.x = ((float) t[0]) * 0.1f;
+ data.y = ((float) t[1]) * 0.1f;
+ data.z = ((float) t[2]) * 0.1f;
+}
+
+
+void FXOS8700Q_mag::getAxis(MotionSensorDataCounts &data) {
+ int16_t acc;
+ uint8_t res[6];
+ readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
+
+ data.x = (res[0] << 8) | res[1];
+ data.y = (res[2] << 8) | res[3];
+ data.z = (res[4] << 8) | res[5];
+}
+
+void FXOS8700Q_mag::readRegs(int addr, uint8_t * data, int len) {
+ char t[1] = {addr};
+ m_i2c.write(m_addr, t, 1, true);
+ m_i2c.read(m_addr, (char *)data, len);
+}
+
+void FXOS8700Q_mag::writeRegs(uint8_t * data, int len) {
+ m_i2c.write(m_addr, (char *)data, len);
+}
+
+
+int16_t FXOS8700Q_mag::getAccAxis(uint8_t addr) {
+ int16_t acc;
+ uint8_t res[2];
+ readRegs(addr, res, 2);
+
+ acc = (res[0] << 8) | res[1];
+
+ return acc;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SensorsLibrary/FXOS8700Q.h Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,148 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* 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 FXOS8700Q_H
+#define FXOS8700Q_H
+
+#include "mbed.h"
+#include "MotionSensor.h"
+// FXOS8700CQ I2C address
+#define FXOS8700CQ_SLAVE_ADDR0 (0x1E<<1) // with pins SA0=0, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR1 (0x1D<<1) // with pins SA0=1, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR2 (0x1C<<1) // with pins SA0=0, SA1=1
+#define FXOS8700CQ_SLAVE_ADDR3 (0x1F<<1) // with pins SA0=1, SA1=1
+// FXOS8700CQ internal register addresses
+#define FXOS8700Q_STATUS 0x00
+#define FXOS8700Q_OUT_X_MSB 0x01
+#define FXOS8700Q_OUT_Y_MSB 0x03
+#define FXOS8700Q_OUT_Z_MSB 0x05
+#define FXOS8700Q_M_OUT_X_MSB 0x33
+#define FXOS8700Q_M_OUT_Y_MSB 0x35
+#define FXOS8700Q_M_OUT_Z_MSB 0x37
+#define FXOS8700Q_WHOAMI 0x0D
+#define FXOS8700Q_XYZ_DATA_CFG 0x0E
+#define FXOS8700Q_CTRL_REG1 0x2A
+#define FXOS8700Q_M_CTRL_REG1 0x5B
+#define FXOS8700Q_M_CTRL_REG2 0x5C
+#define FXOS8700Q_WHOAMI_VAL 0xC7
+
+
+/**
+* MMA8451Q accelerometer example
+*
+* @code
+* #include "mbed.h"
+* #include "FXOS8700Q.h"
+*
+*
+* int main(void) {
+*
+* FXOS8700Q combo( A4, A5, FXOS8700Q_I2C_ADDRESS0);
+* PwmOut rled(LED_RED);
+* PwmOut gled(LED_GREEN);
+* PwmOut bled(LED_BLUE);
+*
+* while (true) {
+* rled1.0 - combo(acc.getAccX());
+* gled1.0 - combo(acc.getAccY());
+* bled1.0 - combo(acc.getAccZ());
+* wait(0.1);
+* }
+* }
+* @endcode
+*/
+
+class FXOS8700Q_acc : public MotionSensor
+{
+public:
+ /**
+ * FXOS8700Q constructor
+ *
+ * @param sda SDA pin
+ * @param sdl SCL pin
+ * @param addr addr of the I2C peripheral
+ */
+
+ FXOS8700Q_acc(PinName sda, PinName scl, int addr);
+
+ /**
+ * FXOS8700Q destructor
+ */
+ ~FXOS8700Q_acc();
+
+ void enable(void);
+ void disable(void);
+ uint32_t sampleRate(uint32_t frequency);
+ uint32_t whoAmI(void);
+ uint32_t dataReady(void);
+ void getX(int16_t * x);
+ void getY(int16_t * y);
+ void getZ(int16_t * z);
+ void getX(float * x);
+ void getY(float * y);
+ void getZ(float * z);
+ void getAxis(MotionSensorDataCounts &data);
+ void getAxis(MotionSensorDataUnits &data);
+
+ void readRegs(int addr, uint8_t * data, int len);
+
+private:
+ I2C m_i2c;
+ int m_addr;
+
+ void writeRegs(uint8_t * data, int len);
+ int16_t getAccAxis(uint8_t addr);
+
+};
+
+class FXOS8700Q_mag : public MotionSensor
+{
+public:
+ FXOS8700Q_mag(PinName sda, PinName scl, int addr);
+
+ /**
+ * FXOS8700Q destructor
+ */
+ ~FXOS8700Q_mag();
+
+ void enable(void);
+ void disable(void);
+ uint32_t sampleRate(uint32_t fequency);
+ uint32_t whoAmI(void);
+ uint32_t dataReady(void);
+ void getX(int16_t * x);
+ void getY(int16_t * y);
+ void getZ(int16_t * z);
+ void getX(float * x);
+ void getY(float * y);
+ void getZ(float * z);
+ void getAxis(MotionSensorDataCounts &data);
+ void getAxis(MotionSensorDataUnits &data);
+
+ void readRegs(int addr, uint8_t * data, int len);
+
+private:
+ I2C m_i2c;
+ int m_addr;
+
+ void writeRegs(uint8_t * data, int len);
+ int16_t getAccAxis(uint8_t addr);
+
+};
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Sep 21 21:42:07 2020 +0000
@@ -0,0 +1,249 @@
+#include "FXAS21002.h"
+#include "FXOS8700Q.h"
+#include "mbed.h"
+#include "CarPWM.h"
+#include "receiver.h"
+#include "Motor.hpp"
+#include "rtos.h"
+
+#include "LED.hpp"
+#include "PIDController.hpp"
+
+#define PI 3.141593
+#define Ts 0.02 // Controller period(Seconds)
+#define END_THRESH 4 //For magnetometer calibration
+#define START_THRESH 10 //For magnetometer calibration
+#define MINIMUM_VELOCITY 15
+#define GYRO_PERIOD 15000 //us
+
+#define MIN -1.5
+#define MAX 1.5
+
+//Control Objetcs
+PwmOut servo(PTD1); // Servo connected to pin PTD3
+Motor *motor = new Motor(PTC3);
+FXOS8700Q_mag mag(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
+FXOS8700Q_acc acc(PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
+FXAS21002 *gyro = new FXAS21002(PTE25, PTE24);
+//Leds Objects
+
+LED *led = new LED(PTD0, PTC4, PTC12, LED_RED, LED_GREEN, LED_BLUE);
+DigitalOut main_led(PTB2);
+//Protocol Objects
+Receiver rcv;
+EthernetInterface eth;
+
+// PID controller parameters and functions
+PIDController *pidController = new PIDController(motor, &servo, gyro);
+
+// Motor and servo variables
+float saved_velocity = 0;
+bool brake = false;
+
+// Magnetometer/Gyro variables and functions
+float max_x = 0, max_y = 0, min_x = 0, min_y = 0, x, y;
+MotionSensorDataUnits mag_data;
+MotionSensorDataCounts mag_raw;
+float processMagAngle();
+void magCal();
+
+// Protocol
+void readProtocol();
+
+int main()
+{
+ // Initializing sensors
+ main_led = 1;
+ acc.enable();
+ gyro->gyro_config(MODE_1);
+
+ // Set initial control configurations
+ motor->setVelocity(0);
+
+ // Protocol initialization
+
+ printf("Initializing Ethernet!\r\n");
+ led->set_leds_color_nxp(LED::RED);
+ led->set_leds_color_main(LED::RED);
+
+ eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR);
+ eth.connect();
+ printf("Protocol initialized! \r\n");
+ gyro->start_measure(GYRO_PERIOD);
+ led->set_leds_color_nxp(LED::BLUE);
+ led->set_leds_color_main(LED::BLUE);
+
+
+ rcv.set_socket();
+
+ main_led = 0;
+
+ pidController->PIDAttach();
+
+ //main loop
+ while (1)
+ {
+ readProtocol();
+ }
+}
+
+void readProtocol()
+{
+ if (!rcv.receive())
+ return;
+ char msg = rcv.get_msg();
+
+ switch (msg)
+ {
+ case NONE:
+ break;
+ case BRAKE:
+ float intensity, b_wait;
+ rcv.get_brake(&intensity, &b_wait);
+ if (!brake)
+ {
+ led->set_leds_color_nxp(LED::YELLOW);
+ motor->stopJogging();
+ setServoPWM(0, servo);
+ pidController->PIDDetach();
+ motor->brakeMotor(intensity, b_wait);
+ pidController->PIDAttach();
+
+ saved_velocity = 0;
+ brake = true;
+ }
+ break;
+ case GYRO_ZERO:
+ gyro->stop_measure();
+ wait(0.05);
+ gyro->start_measure(GYRO_PERIOD);
+ break;
+ case ANG_SET:
+ led->set_leds_color_nxp(LED::PURPLE);
+ //printf("ANG_SET\r\n");
+ pidController->setGyroReference(gyro->get_angle());
+ pidController->initializeController();
+ break;
+ case ANG_RST:
+ //printf("ANG_RST\r\n");
+ pidController->setGyroReference(0);
+ led->set_leds_color_nxp(LED::PURPLE);
+ pidController->initializeController();
+ break;
+ case MAG_ANG_REF:
+ led->set_leds_color_nxp(LED::BLUE);
+ pidController->setTargetAngle(rcv.get_mag_ang_ref() - processMagAngle());
+ break;
+ case ABS_ANG_REF:
+ led->set_leds_color_nxp(LED::GREEN);
+ pidController->setTargetAngle(rcv.get_abs_ang_ref());
+ break;
+ case REL_ANG_REF:
+ led->set_leds_color_nxp(LED::RED);
+ pidController->setTargetAngle(rcv.get_rel_ang_ref() + gyro->get_angle() * PI / 180);
+ break;
+ case GND_VEL:
+ led->set_leds_color_nxp(LED::AQUA);
+ saved_velocity = rcv.get_gnd_vel();
+ if (saved_velocity > 0)
+ {
+ motor->setVelocity(saved_velocity);
+ if (abs(saved_velocity) > MINIMUM_VELOCITY)
+ brake = false;
+ }
+ break;
+ case JOG_VEL:
+ led->set_leds_color_nxp(LED::WHITE);
+ float p, r;
+ rcv.get_jog_vel(&p, &r);
+ if (p == 0 || r == 0)
+ motor->stopJogging();
+ else
+ motor->startJogging(r, p);
+ break;
+ case PID_PARAMS:
+ led->set_leds_color_nxp(LED::WHITE);
+ float arr[4];
+ rcv.get_pid_params(arr);
+ pidController->setPIDConstants(arr[0], arr[1], arr[2], arr[3]);
+
+ break;
+ case MAG_CALIB:
+ led->set_leds_color_nxp(LED::BLUE);
+ float mag[4];
+ rcv.get_mag_calib(mag);
+ max_x = mag[1];
+ max_y = mag[3];
+ min_x = mag[0];
+ min_y = mag[2];
+ //printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]);
+ break;
+ case LED_ON:
+ led->set_leds_color_nxp(LED::WHITE);
+ led->set_leds_color_main(LED::WHITE);
+ main_led = 1;
+ break;
+ case LED_OFF:
+ led->turn_leds_off_nxp();
+ led->turn_leds_off_main();
+ main_led = 0;
+ break;
+ default:
+ //ser.printf("unknown command!\r\n");
+ }
+}
+
+/* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
+/* Function to normalize the magnetometer reading */
+void magCal()
+{
+ //red_led = 0;
+ printf("Starting Calibration");
+ mag.enable();
+ wait(0.01);
+ mag.getAxis(mag_data);
+ float x0 = max_x = min_y = mag_data.x;
+ float y0 = max_y = min_y = mag_data.y;
+ bool began = false;
+ while (!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH))
+ {
+ mag.getAxis(mag_data);
+ if (mag_data.x > max_x)
+ max_x = mag_data.x;
+ if (mag_data.y > max_y)
+ max_y = mag_data.y;
+ if (mag_data.y < min_y)
+ min_y = mag_data.y;
+ if (mag_data.x < min_x)
+ min_x = mag_data.x;
+ if (abs(mag_data.x - x0) > START_THRESH && abs(mag_data.y - y0) > START_THRESH)
+ began = true;
+ printf("began: %d X-X0: %f , Y-Y0: %f \n\r", began, abs(mag_data.x - x0), abs(mag_data.y - y0));
+ }
+ printf("Calibration Completed: X_MAX = %f , Y_MAX = %f , X_MIN = %f and Y_MIN = %f \n\r", max_x, max_y, min_x, min_y);
+}
+
+/* Function to transform the magnetometer reading in angle(rad/s).*/
+float processMagAngle()
+{
+ // printf("starting ProcessMagAngle()\n\r");
+ float mag_lpf = 0;
+ Timer t1;
+ for (int i = 0; i < 20; i++)
+ {
+ //printf("\r\n");
+ //wait(0.1);
+ t1.start();
+ __disable_irq();
+ mag.getAxis(mag_data);
+ __enable_irq();
+ t1.stop();
+ x = 2 * (mag_data.x - min_x) / float(max_x - min_x) - 1;
+ y = 2 * (mag_data.y - min_y) / float(max_y - min_y) - 1;
+ mag_lpf = mag_lpf + (-atan2(y, x));
+ wait(0.015);
+ }
+ // wait(20*0.01);
+ // printf("Finished ProcessMagAngle() %d \n\r",t1.read_us());
+ return mag_lpf / 20;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Mon Sep 21 21:42:07 2020 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#162b12aea5f2
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Sep 21 21:42:07 2020 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/99a22ba036c9 \ No newline at end of file