Skip to content

Commit 34baeff

Browse files
gbr1gbr1
authored
Merge pull request #8 from arduino-libraries/devel
0.3.0
2 parents 86dcea6 + fd4d760 commit 34baeff

10 files changed

+186
-39
lines changed

‎examples/firmware_01/firmware_01.ino renamed to ‎examples/firmware/firmware.ino

+48-11
Original file line numberDiff line numberDiff line change
@@ -28,15 +28,17 @@ uint8_t code;
2828
uint8_t label;
2929
uint8_t control_type;
3030
uint8_t msg_size;
31-
uint8_t ack_required=0;
32-
bool ack_check=false;
33-
uint8_t ack_code=0;
31+
uint8_t ack_required = 0;
32+
bool ack_check = false;
33+
uint8_t ack_code = 0;
34+
uint8_t behaviours;
3435

35-
unsigned long tmotor=0;
36-
unsigned long tsend=0;
37-
unsigned long tsensor=0;
38-
unsigned long timu=0;
39-
unsigned long tack=0;
36+
unsigned long tmotor = 0;
37+
unsigned long tsend = 0;
38+
unsigned long tsensor = 0;
39+
unsigned long timu = 0;
40+
unsigned long tack = 0;
41+
unsigned long tbehaviours = 0;
4042

4143

4244
float left, right, value;
@@ -51,6 +53,10 @@ float kp, ki, kd;
5153
float x, y, theta;
5254

5355
uint8_t servo_A, servo_B;
56+
float position_left, position_right;
57+
58+
int counter_version = 9;
59+
uint8_t version[3];
5460

5561

5662
void setup(){
@@ -62,7 +68,7 @@ void setup(){
6268
line.begin();
6369
tof.begin();
6470

65-
uint8_t version[3];
71+
6672
alvik.getVersion(version[0], version[1], version[2]);
6773
msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
6874
alvik.serial->write(packeter.msg,msg_size);
@@ -77,6 +83,7 @@ void setup(){
7783
tsensor=millis();
7884
timu=millis();
7985
tack=millis();
86+
tbehaviours=millis();
8087
}
8188

8289
void loop(){
@@ -132,6 +139,14 @@ void loop(){
132139
}
133140
}
134141
break;
142+
143+
144+
case 'A':
145+
packeter.unpacketC2F(code,position_left, position_right);
146+
alvik.disableKinematicsMovement();
147+
alvik.setPosition(position_left, position_right);
148+
break;
149+
135150

136151
case 'S':
137152
packeter.unpacketC2B(code,servo_A,servo_B);
@@ -181,6 +196,17 @@ void loop(){
181196
ack_check = false;
182197
}
183198
break;
199+
200+
case 'B':
201+
packeter.unpacketC1B(code, behaviours);
202+
switch (behaviours){
203+
case 1:
204+
alvik.setBehaviour(LIFT_ILLUMINATOR, true);
205+
break;
206+
default:
207+
alvik.setBehaviour(ALL_BEHAVIOURS, false);
208+
}
209+
break;
184210
}
185211
}
186212

@@ -205,7 +231,7 @@ void loop(){
205231
break;
206232
case 3:
207233
if (tof.update_rois()){
208-
msg_size = packeter.packetC7I('f', tof.getLeft(), tof.getCenterLeft(), tof.getCenter(), tof.getCenterRight(), tof.getRight(), tof.get_min_range_top_mm(), tof.get_max_range_bottom_mm());
234+
msg_size = packeter.packetC7I('f', tof.getLeft(), tof.getCenterLeft(), tof.getCenter(), tof.getCenterRight(), tof.getRight(), tof.getTop(), tof.getBottom());
209235
alvik.serial->write(packeter.msg,msg_size);
210236
}
211237
break;
@@ -221,7 +247,7 @@ void loop(){
221247
}
222248

223249
// motors update & publish
224-
if (millis()-tmotor>20){
250+
if (millis()-tmotor>=20){
225251
tmotor=millis();
226252
alvik.updateMotors();
227253
alvik.updateKinematics();
@@ -242,6 +268,12 @@ void loop(){
242268
// acknowledge
243269
if (millis()-tack > 100){
244270
tack = millis();
271+
if (counter_version>0){
272+
counter_version--;
273+
alvik.getVersion(version[0], version[1], version[2]);
274+
msg_size = packeter.packetC3B(0x7E, version[0], version[1], version[2]);
275+
alvik.serial->write(packeter.msg,msg_size);
276+
}
245277
if (ack_check && alvik.isTargetReached()){
246278
if (ack_required == MOVEMENT_ROTATE){
247279
msg_size = packeter.packetC1B('x', 'R');
@@ -256,6 +288,11 @@ void loop(){
256288
alvik.serial->write(packeter.msg, msg_size);
257289
}
258290

291+
if (millis()-tbehaviours > 100){
292+
tbehaviours = millis();
293+
alvik.updateBehaviours();
294+
}
295+
259296
// imu update
260297
if (millis()-timu>10){
261298
timu=millis();

‎examples/firmware_01/platform_config_custom.h renamed to ‎examples/firmware/platform_config_custom.h

File renamed without changes.

‎library.properties

+2-2
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
1-
name=Arduino Alvik Carrier
2-
version=0.2.0
1+
name=Arduino_AlvikCarrier
2+
version=0.3.0
33
author=Arduino, Giovanni di Dio Bruno, Lucio Rossi
44
maintainer=Arduino <[email protected]>
55
sentence=Library and firmware for Arduino Alvik Carrier board

‎src/Arduino_AlvikCarrier.cpp

+83-1
Original file line numberDiff line numberDiff line change
@@ -133,6 +133,8 @@ int Arduino_AlvikCarrier::begin(){
133133
if (beginImu()!=0){
134134
errorLed(ERROR_IMU);
135135
}
136+
137+
beginBehaviours();
136138

137139

138140
return 0;
@@ -171,15 +173,18 @@ void Arduino_AlvikCarrier::updateAPDS(){
171173
}
172174

173175
void Arduino_AlvikCarrier::setIlluminator(uint8_t value){
176+
illuminator_state=value;
174177
digitalWrite(APDS_LED, value);
175178
}
176179

177180
void Arduino_AlvikCarrier::enableIlluminator(){
178181
setIlluminator(HIGH);
182+
prev_illuminator_state = true;
179183
}
180184

181185
void Arduino_AlvikCarrier::disableIlluminator(){
182186
setIlluminator(LOW);
187+
prev_illuminator_state = false;
183188
}
184189

185190
int Arduino_AlvikCarrier::getRed(){
@@ -540,7 +545,18 @@ void Arduino_AlvikCarrier::setLeds(const uint32_t red, const uint32_t green, con
540545

541546
void Arduino_AlvikCarrier::setAllLeds(const uint8_t value){
542547
setLedBuiltin(value&1);
543-
setIlluminator((value>>1)&1);
548+
//setIlluminator((value>>1)&1);
549+
if ((value>>1)&1){
550+
if ((behaviours|=1 == 1)&&isLifted()){
551+
prev_illuminator_state = true;
552+
}
553+
else{
554+
enableIlluminator();
555+
}
556+
}
557+
else{
558+
disableIlluminator();
559+
}
544560
setLedLeftRed(((value>>2)&1));
545561
setLedLeftGreen(((value>>3)&1));
546562
setLedLeftBlue(((value>>4)&1));
@@ -811,3 +827,69 @@ bool Arduino_AlvikCarrier::isTargetReached(){
811827
uint8_t Arduino_AlvikCarrier::getKinematicsMovement(){
812828
return kinematics_movement;
813829
}
830+
831+
832+
833+
/******************************************************************************************************/
834+
/* Behaviours */
835+
/******************************************************************************************************/
836+
void Arduino_AlvikCarrier::beginBehaviours(){
837+
prev_illuminator_state = illuminator_state;
838+
behaviours = 0;
839+
first_lift = true;
840+
}
841+
842+
843+
void Arduino_AlvikCarrier::updateBehaviours(){
844+
if (behaviours|=1 == 1){
845+
/*
846+
if (isLifted()&&first_lift){
847+
first_lift = false;
848+
prev_illuminator_state = illuminator_state;
849+
disableIlluminator();
850+
}
851+
if (isLifted()&&!first_lift) {
852+
if (prev_illuminator_state!=0){
853+
disableIlluminator();
854+
}
855+
}
856+
if (!isLifted()&&!first_lift){
857+
if (prev_illuminator_state!=0){
858+
//first_lift = true;
859+
enableIlluminator();
860+
}
861+
}
862+
*/
863+
if (isLifted()&&first_lift){
864+
//disableIlluminator();
865+
setIlluminator(LOW);
866+
first_lift=false;
867+
}
868+
else{
869+
if (!isLifted()){
870+
setIlluminator(prev_illuminator_state);
871+
}
872+
if (!isLifted()&&!first_lift){
873+
first_lift = true;
874+
}
875+
}
876+
}
877+
}
878+
879+
void Arduino_AlvikCarrier::setBehaviour(const uint8_t behaviour, const bool enable){
880+
if (enable){
881+
behaviours |= behaviour;
882+
}
883+
else{
884+
behaviours &= ~behaviour;
885+
}
886+
}
887+
888+
bool Arduino_AlvikCarrier::isLifted(){
889+
if (getProximity()>LIFT_THRESHOLD){
890+
return true;
891+
}
892+
else{
893+
return false;
894+
}
895+
}

‎src/Arduino_AlvikCarrier.h

+11-1
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@ class Arduino_AlvikCarrier{
3737

3838
APDS9960 * apds9960;
3939
int bottom_red, bottom_green, bottom_blue, bottom_clear, bottom_proximity;
40+
bool illuminator_state;
4041

4142

4243
Servo * servo_A;
@@ -79,6 +80,12 @@ class Arduino_AlvikCarrier{
7980
PidController * move_pid;
8081

8182

83+
bool prev_illuminator_state;
84+
uint8_t behaviours;
85+
bool first_lift;
86+
87+
88+
8289

8390
public:
8491
Kinematics * kinematics;
@@ -238,7 +245,10 @@ class Arduino_AlvikCarrier{
238245
bool isTargetReached(); // get true if a movement is accomplished
239246
uint8_t getKinematicsMovement(); // get which kind of motion is running in kinematic control
240247

241-
248+
void beginBehaviours();
249+
void updateBehaviours();
250+
void setBehaviour(const uint8_t behaviour, const bool enable);
251+
bool isLifted();
242252
};
243253

244254
#endif

‎src/definitions/pinout_definitions.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -95,7 +95,7 @@
9595
// Uart
9696
#define UART_TX PA9
9797
#define UART_RX PA10
98-
#define UART_BAUD 115200
98+
#define UART_BAUD 460800
9999

100100

101101
// Errors

‎src/definitions/robot_definitions.h

+9-1
Original file line numberDiff line numberDiff line change
@@ -60,6 +60,14 @@ const float MOTOR_RATIO = MOTOR_CPR*MOTOR_GEAR_RATIO;
6060
#define MOVEMENT_MOVE 2
6161

6262

63+
#define LIFT_THRESHOLD 80
64+
65+
#define ALL_BEHAVIOURS 255
66+
#define LIFT_ILLUMINATOR 1
67+
68+
69+
70+
6371

6472

6573
// Sensor fusioning parameters
@@ -80,7 +88,7 @@ const float MOTION_FX_PERIOD = (1000U / MOTION_FX_FREQ);
8088
// Library version
8189
#define VERSION_BYTE_HIGH 0
8290
#define VERSION_BYTE_MID 2
83-
#define VERSION_BYTE_LOW 0
91+
#define VERSION_BYTE_LOW 1
8492

8593

8694

‎src/motor_control/motor_control.cpp

+23-15
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,8 @@ MotorControl::MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp
4242
angle = 0.0;
4343
reference = 0.0;
4444

45+
return_flag = false;
46+
4547
trip = 0.0;
4648
iterations = 0.0;
4749
start_value = 0.0;
@@ -111,22 +113,28 @@ float MotorControl::getRPM(){
111113

112114

113115
bool MotorControl::setRPM(const float ref){
114-
if ((ref<=MOTOR_LIMIT)&&(ref>=-MOTOR_LIMIT)){
115-
reference = ref;
116-
if (control_mode==CONTROL_MODE_LINEAR){
117-
start_value=interpolation;
118-
end_value=reference;
119-
trip=0.0;
120-
iterations=abs(end_value-start_value)/step_size;
121-
step=1.0/iterations;
122-
step_index=0;
123-
}
124-
else if(control_mode==CONTROL_MODE_NORMAL){
125-
vel_pid->setReference(reference);
126-
}
127-
return true;
116+
reference = ref;
117+
return_flag = true;
118+
if (ref>MOTOR_LIMIT){
119+
reference=MOTOR_LIMIT;
120+
return_flag = false;
121+
}
122+
if (ref<-MOTOR_LIMIT){
123+
reference=-MOTOR_LIMIT;
124+
return_flag = false;
125+
}
126+
if (control_mode==CONTROL_MODE_LINEAR){
127+
start_value=interpolation;
128+
end_value=reference;
129+
trip=0.0;
130+
iterations=abs(end_value-start_value)/step_size;
131+
step=1.0/iterations;
132+
step_index=0;
133+
}
134+
else if(control_mode==CONTROL_MODE_NORMAL){
135+
vel_pid->setReference(reference);
128136
}
129-
return false;
137+
return return_flag;
130138
}
131139

132140
void MotorControl::update(){

‎src/motor_control/motor_control.h

+2
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,8 @@ class MotorControl{
2929
private:
3030

3131
uint8_t control_mode;
32+
bool return_flag;
33+
3234

3335
float kp;
3436
float ki;

0 commit comments

Comments
 (0)