Skip to content

Commit

Permalink
Big Random WIP
Browse files Browse the repository at this point in the history
  • Loading branch information
@Nodraak
Nodraak committed Jul 11, 2017
1 parent 2062551 commit 6c62fc3
Show file tree
Hide file tree
Showing 17 changed files with 428 additions and 39 deletions.
18 changes: 18 additions & 0 deletions Deoxys/Galib/QBouge/MotionController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,21 @@
#ifndef MOTION_CONTROLLER_H_INLCUDED
#define MOTION_CONTROLLER_H_INLCUDED

/*
We should extract the stuff related to QEI Positionning out of MotionController
QeiPosition
qei l, r
pos
angle
speed, speed_ang
MotionController
Motor l, r
Pid dist, angle
current order
*/

/*
The MotionController have the responsability to control the motors to move
the robot to the specified coordinates (it execute orders: position, angle,
Expand Down Expand Up @@ -39,6 +54,9 @@
#define MC_TARGET_TOLERANCE_ANGLE DEG2RAD(2) // unit: rad - Gali IX 7 - ESEO 2 deg
#define MC_TARGET_TOLERANCE_ANG_SPEED DEG2RAD(2) // unit: rad/sec - ESEO 3 deg

// todo print this
// max error due to angle tolerance (sqrt(100**2+(130+140)**2) * sin(MC_TARGET_TOLERANCE_ANG_SPEED)) == 10

// Max speed

#define PID_DIST_MAX_OUPUT 0.8
Expand Down
1 change: 1 addition & 0 deletions Deoxys/Galib/QBouge/Motor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
// Note: be sure to have a steep increase (low PWM_STEP_1_AFTER_X_SEC value),
// otherwise it might fucked up the PID settings
#define PWM_STEP_1_AFTER_X_SEC 0.25
// This is acceleration and should not be constant, but be incremented -> https://www.rcva.fr/10-ans-dexperience/4/
#define PWM_STEP (1.0/(ASSERV_FPS*PWM_STEP_1_AFTER_X_SEC))

#define PWM_IS_ALMOST_ZERO (PWM_STEP/2) // Pwm under this value is considered to be 0
Expand Down
10 changes: 6 additions & 4 deletions Deoxys/Galib/QEntreQSort/Actuators.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ void ServoActuator::close(void) {
Ax12Actuator::Ax12Actuator(const char *name, Ax12Driver *ax12, uint8_t id, uint16_t retracted, uint16_t neutral, uint16_t extended) :
name_(name), ax12_(ax12), id_(id), retracted_(retracted), neutral_(neutral), extended_(extended)
{
this->retract();
// this->retract();
}

void Ax12Actuator::print(Debug *debug, int depth) {
Expand Down Expand Up @@ -219,9 +219,9 @@ ArmActuator::ArmActuator(
ServoActuator clamp,
BooleanActuator pump
) : height_(height), vert_(vert), horiz_(horiz), clamp_(clamp), pump_(pump) {
height_.extend();
vert_.extend();
horiz_.extend();
// height_.extend();
// vert_.extend();
// horiz_.extend();
clamp_.open();
pump_.off();
}
Expand Down Expand Up @@ -328,6 +328,7 @@ void OneSideCylindersActuators::print(Debug *debug, int depth) {
arm_.print(debug, depth+1);
flap_.print(debug, depth+1);
prograde_dispenser_.print(debug, depth+1);
// print crs ?
}

void OneSideCylindersActuators::set(t_act act, float val) {
Expand All @@ -343,6 +344,7 @@ void OneSideCylindersActuators::set(t_act act, float val) {
flap_.set(act, val);
if (act & ACT_ACTUATOR_PROG)
prograde_dispenser_.set(act, val);
// set crs ?
}

void OneSideCylindersActuators::activate(t_act act) {
Expand Down
1 change: 1 addition & 0 deletions Deoxys/Galib/QEntreQSort/ColorSensor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ void ColorSensor::update(void) {
e_color ColorSensor::get_val(void) {
e_color new_color;

// todo define
if ((last_avg_g_ > 0.058) && (last_avg_b_ > 0.058))
new_color = COLOR_WHITE;
else if ((last_avg_g_ > 0.058) && (last_avg_b_ < 0.058))
Expand Down
12 changes: 12 additions & 0 deletions Deoxys/Galib/QEntreQSort/ColorSensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@
last RAW2AVG_WINDOW (50) raw values. we work only on these averaged values.
fancy algorithms with a calibraton phase has been tried, but the dumb
version actually works better.
Blue is not recognized !!
*/


Expand Down Expand Up @@ -65,3 +67,13 @@ class ColorSensor {

#endif // #ifndef SENSORS_H_INCLUDED
#endif // #ifdef IAM_QENTRESORT

/*
const char *color2str[] = {
"COLOR_UNKNOWN",
"COLOR_NOT_CONFIDENT",
"COLOR_BLUE",
"COLOR_YELLOW",
"COLOR_WHITE"
};
*/
41 changes: 15 additions & 26 deletions Deoxys/Galib/common/Messenger.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,9 +68,7 @@ const char *e2s_message_type(Message::e_message_type msg)
*/

Message::Message(void) {
id = MT_empty;
len = 0;
memset(payload.raw_data, 0, 8);
Message(MT_empty);
}

Message::Message(e_message_type id_, unsigned int len_, u_payload payload_) {
Expand Down Expand Up @@ -180,16 +178,18 @@ int CanMessenger::read_msg(Message *dest) {
return ret;
}

void CanMessenger::set_silent(bool enable) {
can_.monitor(enable);
}
// Bug fixed by implementation
// void CanMessenger::set_silent(bool enable) {
// can_.monitor(enable);
// }

void CanMessenger::leave_the_bus_for_a_moment(void) {
this->set_silent(true);
Thread::wait(3); // ms
// 1/(500*1000) * (128*11) == 2.8 ms - bus off recovery time (let other boards initialise their CAN)
this->set_silent(false);
}
// Bug fixed by implementation
// void CanMessenger::leave_the_bus_for_a_moment(void) {
// this->set_silent(true);
// Thread::wait(3); // ms
// // 1/(500*1000) * (128*11) == 2.8 ms - bus off recovery time (let other boards initialise their CAN)
// this->set_silent(false);
// }

int CanMessenger::on_receive_add(Message::e_message_type type, Callback<void(void*)> cb) {
if (or_count_ == ON_RECEIVE_SLOT_COUNT)
Expand All @@ -209,24 +209,13 @@ int CanMessenger::on_receive_add(Message::e_message_type type, Callback<void(voi

#ifdef IAM_QREFLECHI
int CanMessenger::send_msg_CQR_ping(void) {
return this->send_msg(Message(Message::MT_CQR_ping, 0, (Message::u_payload){}));
// TODO inline this
return this->send_msg(Message::make_ping());
}
#endif

int CanMessenger::send_msg_pong(void) {
Message::e_message_type message_type;

#ifdef IAM_QBOUGE
message_type = Message::MT_CQB_pong;
#endif
#ifdef IAM_QREFLECHI
message_type = Message::MT_CQR_pong;
#endif
#ifdef IAM_QENTRESORT
message_type = Message::MT_CQES_pong;
#endif

return this->send_msg(Message(message_type, 0, (Message::u_payload){}));
return this->send_msg(Message::make_pong());
}

#ifdef IAM_QREFLECHI
Expand Down
26 changes: 26 additions & 0 deletions Deoxys/Galib/common/Messenger.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include "common/OrdersFIFO.h"
#include "common/utils.h"

// todo assert all size

/*
Can bus baud rate, in Hz.
Expand Down Expand Up @@ -103,6 +104,7 @@ class Message {
MT_CQB_finished = 510,
MT_CQES_finished = 511,
MT_CQR_finished = 512,
// *_I_am_finished order_id -> much better IMO

MT_CQB_next_order_request = 520,
MT_CQES_next_order_request = 521,
Expand Down Expand Up @@ -224,8 +226,32 @@ class Message {
*/

Message(void); // Receive. Fills the attributes with default placeholders.
Message(e_message_type id_) // len and payload are 0 and NULL
{
Message(id_, 0, (Message::u_payload){});
};
Message(e_message_type id_, unsigned int len_, u_payload payload_); // Send.

static Message make_ping(void) {
return Message(Message::MT_CQR_ping);
};
static Message make_pong(void) {
Message::e_message_type message_type;

#ifdef IAM_QBOUGE
message_type = Message::MT_CQB_pong;
#endif
#ifdef IAM_QREFLECHI
message_type = Message::MT_CQR_pong;
#endif
#ifdef IAM_QENTRESORT
message_type = Message::MT_CQES_pong;
#endif

return Message(message_type);
};


/*
Class Attributes
*/
Expand Down
52 changes: 46 additions & 6 deletions Deoxys/Galib/common/com.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,6 +134,35 @@ void com_handle_can(
);
break;

case Message::MT_CQES_sensor_sharp_front:
// debug->printf("[CAN] MT_CQES_sensor_sharp_front %d %d %d\n",
// rec_msg.payload.CQES_sensor_sharp_front.left,
// rec_msg.payload.CQES_sensor_sharp_front.middle,
// rec_msg.payload.CQES_sensor_sharp_front.right
// );

// mc->sensor_handle(SHARP_FRONT_LEFT, rec_msg.payload.CQES_sensor_sharp_front.left);
// mc->sensor_handle(SHARP_FRONT_MIDDLE, rec_msg.payload.CQES_sensor_sharp_front.middle);
// mc->sensor_handle(SHARP_FRONT_RIGHT, rec_msg.payload.CQES_sensor_sharp_front.right);

break;

case Message::MT_CQES_sensor_sharp_back:
// debug->printf("[CAN] MT_CQES_sensor_sharp_back %d %d\n",
// rec_msg.payload.CQES_sensor_sharp_back.left,
// rec_msg.payload.CQES_sensor_sharp_back.left
// );
break;

case Message::MT_CQES_sensor_ultrasound:
// debug->printf("[CAN] \t\t\t\t MT_CQES_sensor_ultrasound %d %d %d %d\n",
// rec_msg.payload.CQES_sensor_ultrasound.front_left,
// rec_msg.payload.CQES_sensor_ultrasound.front_right,
// rec_msg.payload.CQES_sensor_ultrasound.back_left,
// rec_msg.payload.CQES_sensor_ultrasound.back_right
// );
break;

case Message::MT_CQR_order:
// todo ack if ok, else send error
// todo sync with CQES
Expand Down Expand Up @@ -165,8 +194,11 @@ void com_handle_can(

#ifdef IAM_QREFLECHI
case Message::MT_CQB_next_order_request:
// debug->printf("\t-> MT_CQB_next_order_request\n");
if (orders->size() == 0)
debug->printf("\t-> orders->size() == 0\n");
{
// debug->printf("\t-> orders->size() == 0\n");
}
// todo send a shut up order
else
{
Expand All @@ -180,11 +212,11 @@ void com_handle_can(

case Message::MT_CQB_I_am_doing:
case Message::MT_CQES_I_am_doing:
debug->printf(
"\t-> I am doing %s %s\n",
rec_msg.payload.I_am_doing.i_am,
e2s_order_exe_type[rec_msg.payload.I_am_doing.order]
);
// debug->printf(
// "\t-> I am doing %s %s\n",
// rec_msg.payload.I_am_doing.i_am,
// e2s_order_exe_type[rec_msg.payload.I_am_doing.order]
// );
break;

case Message::MT_CQB_MC_pos_angle:
Expand All @@ -196,6 +228,14 @@ void com_handle_can(
);
break;

case Message::MT_CQB_MC_encs:
debug->printf(
"\t-> enc %d %d\n",
rec_msg.payload.CQB_MC_encs.enc_l,
rec_msg.payload.CQB_MC_encs.enc_r
);
break;

case Message::MT_CQES_sensor_sharp_front:
case Message::MT_CQES_sensor_sharp_back:
case Message::MT_CQES_sensor_ultrasound:
Expand Down
Loading

0 comments on commit 6c62fc3

Please sign in to comment.