amiro-os / devices / DiWheelDrive / userthread.hpp @ 10bf9cc0
History | View | Annotate | Download (4.986 KB)
1 |
#ifndef AMIRO_USERTHREAD_H_
|
---|---|
2 |
#define AMIRO_USERTHREAD_H_
|
3 |
|
4 |
#include <ch.hpp> |
5 |
#include <amiroosconf.h> |
6 |
#include <amiro/Color.h> |
7 |
// #include "global.hpp"
|
8 |
// #include "linefollow.hpp"
|
9 |
#include <cmath> |
10 |
|
11 |
// Speed when driving towards the docking station
|
12 |
#define CHARGING_SPEED 5 |
13 |
#define DETECTION_SPEED 10 |
14 |
#define DIST_THRESH 100 |
15 |
#define RELEASE_COUNT 200 |
16 |
// Thresh to determain how much update steps should pass while alining
|
17 |
// #define MAX_CORRECTION_STEPS 200
|
18 |
#define DOCKING_CORRECTION_TIMEOUT 200 |
19 |
#define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT |
20 |
|
21 |
// Thresh for wheel proxy sensors, when summed values fall below the state changes
|
22 |
// #define PROXY_WHEEL_THRESH 18000
|
23 |
// Thresh for detecting obsticles
|
24 |
// #define PROXY_RING_THRESH 15000
|
25 |
|
26 |
// #define PUSH_BACK_COUNT 5
|
27 |
#define PUSH_BACK_TIMEOUT 5 |
28 |
// Thresh for how long (update steps) the front sensors are allowed to detect white
|
29 |
// #define WHITE_COUNT_THRESH 150
|
30 |
#define WHITE_DETETION_TIMEOUT 150 |
31 |
// #define RING_PROX_COUNT_THRESH 1000
|
32 |
#define RING_PROX_DETECTION_TIMEOUT 800 |
33 |
// Rotation around 180 degrees in microradian
|
34 |
// #define ROTATION_180 3141592
|
35 |
#define ROTATION_180 3141592 |
36 |
// Rotation around -20 degrees in microradian
|
37 |
#define ROTATION_20 -349065 |
38 |
#define ROTATION_DURATION 10000 |
39 |
|
40 |
#define RING_PROX_FRONT_THRESH 18000 |
41 |
#define PROX_MAX_VAL 65430 |
42 |
|
43 |
// Threshold for failing to dock
|
44 |
#define DOCKING_ERROR_THRESH 3 |
45 |
#define CAN_TRANSMIT_STATE_THRESH 50 |
46 |
|
47 |
|
48 |
namespace amiro {
|
49 |
|
50 |
|
51 |
class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE> |
52 |
{ |
53 |
|
54 |
// Messages which can be received and trigger state changes
|
55 |
public:
|
56 |
|
57 |
// States of user thread state machine
|
58 |
enum states : int8_t{
|
59 |
IDLE = 0,
|
60 |
FOLLOW_LINE = 1,
|
61 |
DETECT_STATION = 2,
|
62 |
REVERSE = 3,
|
63 |
PUSH_BACK = 4,
|
64 |
CHECK_POSITIONING = 5,
|
65 |
CHECK_VOLTAGE = 6,
|
66 |
CHARGING = 7,
|
67 |
RELEASE = 8,
|
68 |
RELEASE_TO_CORRECT = 9,
|
69 |
CORRECT_POSITIONING = 10,
|
70 |
TURN = 12,
|
71 |
INACTIVE = 13,
|
72 |
CALIBRATION = 14,
|
73 |
CALIBRATION_CHECK = 15,
|
74 |
DOCKING_ERROR = -1,
|
75 |
REVERSE_TIMEOUT_ERROR = -2,
|
76 |
CALIBRATION_ERROR = -3,
|
77 |
WHITE_DETECTION_ERROR = -4,
|
78 |
PROXY_DETECTION_ERROR = -5,
|
79 |
NO_CHARGING_POWER_ERROR = -6,
|
80 |
UNKNOWN_STATE_ERROR = -7
|
81 |
}; |
82 |
|
83 |
struct ut_counter{
|
84 |
int whiteCount = 0; |
85 |
int ringProxCount = 0; |
86 |
// int correctionCount = 0;
|
87 |
int errorCount = 0; |
88 |
int stateCount = 0; |
89 |
int stepCount = 0; |
90 |
uint32_t stateTime = 0;
|
91 |
}; |
92 |
|
93 |
struct proxy_ctrl {
|
94 |
int threshLow = 100; |
95 |
int threshMid = 500; |
96 |
int threshHigh = 1500; |
97 |
// int threshHigh = 1500;
|
98 |
int penalty = 100000; |
99 |
int pFactor = 310000; |
100 |
int staticCont = 0; |
101 |
}; |
102 |
|
103 |
|
104 |
struct bottom_prox_calibration {
|
105 |
bool calibrateBlack = true; |
106 |
uint32_t buf = 0;
|
107 |
uint8_t meanWindow = 150;
|
108 |
}; |
109 |
|
110 |
// static const struct ut_counter emptyUtCount;
|
111 |
ut_counter utCount; |
112 |
proxy_ctrl pCtrl; |
113 |
bottom_prox_calibration proxCalib; |
114 |
explicit UserThread();
|
115 |
|
116 |
virtual ~UserThread();
|
117 |
|
118 |
virtual msg_t main();
|
119 |
|
120 |
private:
|
121 |
void setRpmSpeed(const int (&rpmSpeed)[2]); |
122 |
void setRpmSpeedFuzzy(const int (&rpmSpeed)[2]); |
123 |
void lightOneLed(Color color, int idx); |
124 |
void lightAllLeds(Color color);
|
125 |
/**
|
126 |
* Uses light ring indicate the state of charge.
|
127 |
*/
|
128 |
void showChargingState();
|
129 |
void checkForMotion();
|
130 |
bool checkPinVoltage();
|
131 |
bool checkPinEnabled();
|
132 |
int getProxyRingSum();
|
133 |
|
134 |
/**
|
135 |
* Check sectors around and stop if a thresh in one sector is detected.
|
136 |
*/
|
137 |
void preventCollision(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]); |
138 |
|
139 |
/**
|
140 |
* Same as prevent collision but also lowers the speed when object is detected.
|
141 |
*/
|
142 |
void proxSectorSpeedCorrection(int (&rpmSpeed)[2], uint16_t (&proxVals)[8]); |
143 |
void getProxySectorVals(uint16_t (&proxVals)[8], uint16_t (&sProx)[8]); |
144 |
void getMaxFrontSectorVal(uint16_t (&sProx)[8], int32_t &sPMax); |
145 |
void chargeAsLED();
|
146 |
|
147 |
/**
|
148 |
* Returns true when front sensors reaching high values
|
149 |
* and all others are low. This indicates that the loading station is ahead.
|
150 |
* If other sensores are blocked too, indicates that someone grabs the robot.
|
151 |
*/
|
152 |
bool checkFrontalObject();
|
153 |
|
154 |
/**
|
155 |
* Check if current position changes when the wheel are deactivated.
|
156 |
*
|
157 |
* When AMiRo drives towards the loading station, it stops when a specific marker is reached.
|
158 |
* In order to validate that the AMiRo is correctly positioned in the loading station
|
159 |
* the wheels are turned off. When the position remains the same the docking procedure
|
160 |
* was successful (return 1) otherwise a correction is needed (return 0).
|
161 |
*/
|
162 |
int checkDockingSuccess();
|
163 |
|
164 |
// State Variables
|
165 |
states prevState = states::IDLE; |
166 |
states currentState = states::IDLE; |
167 |
states newState = states::IDLE; |
168 |
|
169 |
bool continue_on_obstacle = true; |
170 |
uint16_t rProx[8]; // buffer for ring proxy values |
171 |
int rpmSpeed[2] = {0}; |
172 |
int stop[2] = {0}; |
173 |
int turn[2] = {5,-5}; |
174 |
|
175 |
}; |
176 |
|
177 |
} // end of namespace amiro
|
178 |
|
179 |
#endif // AMIRO_USERTHREAD_H_ |