amiro-os / devices / DiWheelDrive / userthread.hpp @ 10bf9cc0
History | View | Annotate | Download (4.986 KB)
1 | 58fe0e0b | Thomas Schöpping | #ifndef AMIRO_USERTHREAD_H_
|
---|---|---|---|
2 | #define AMIRO_USERTHREAD_H_
|
||
3 | |||
4 | #include <ch.hpp> |
||
5 | #include <amiroosconf.h> |
||
6 | 181f2892 | galberding | #include <amiro/Color.h> |
7 | 10bf9cc0 | galberding | // #include "global.hpp"
|
8 | // #include "linefollow.hpp"
|
||
9 | #include <cmath> |
||
10 | 181f2892 | galberding | |
11 | // Speed when driving towards the docking station
|
||
12 | #define CHARGING_SPEED 5 |
||
13 | 61544eee | galberding | #define DETECTION_SPEED 10 |
14 | #define DIST_THRESH 100 |
||
15 | #define RELEASE_COUNT 200 |
||
16 | 181f2892 | galberding | // Thresh to determain how much update steps should pass while alining
|
17 | 10bf9cc0 | galberding | // #define MAX_CORRECTION_STEPS 200
|
18 | #define DOCKING_CORRECTION_TIMEOUT 200 |
||
19 | #define REVERSE_DOCKING_TIMEOUT 2*DOCKING_CORRECTION_TIMEOUT |
||
20 | |||
21 | 181f2892 | galberding | // Thresh for wheel proxy sensors, when summed values fall below the state changes
|
22 | 10bf9cc0 | galberding | // #define PROXY_WHEEL_THRESH 18000
|
23 | e2002d0e | galberding | // Thresh for detecting obsticles
|
24 | 10bf9cc0 | galberding | // #define PROXY_RING_THRESH 15000
|
25 | 27d4e1fa | galberding | |
26 | 10bf9cc0 | galberding | // #define PUSH_BACK_COUNT 5
|
27 | #define PUSH_BACK_TIMEOUT 5 |
||
28 | 181f2892 | galberding | // Thresh for how long (update steps) the front sensors are allowed to detect white
|
29 | 10bf9cc0 | galberding | // #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 | 181f2892 | galberding | // Rotation around 180 degrees in microradian
|
34 | 10bf9cc0 | galberding | // #define ROTATION_180 3141592
|
35 | 181f2892 | galberding | #define ROTATION_180 3141592 |
36 | // Rotation around -20 degrees in microradian
|
||
37 | #define ROTATION_20 -349065 |
||
38 | #define ROTATION_DURATION 10000 |
||
39 | |||
40 | 61544eee | galberding | #define RING_PROX_FRONT_THRESH 18000 |
41 | #define PROX_MAX_VAL 65430 |
||
42 | 58fe0e0b | Thomas Schöpping | |
43 | 27d4e1fa | galberding | // Threshold for failing to dock
|
44 | 10bf9cc0 | galberding | #define DOCKING_ERROR_THRESH 3 |
45 | #define CAN_TRANSMIT_STATE_THRESH 50 |
||
46 | |||
47 | 27d4e1fa | galberding | |
48 | 58fe0e0b | Thomas Schöpping | namespace amiro {
|
49 | |||
50 | 181f2892 | galberding | |
51 | 58fe0e0b | Thomas Schöpping | class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE> |
52 | { |
||
53 | 181f2892 | galberding | |
54 | // Messages which can be received and trigger state changes
|
||
55 | public:
|
||
56 | |||
57 | // States of user thread state machine
|
||
58 | 10bf9cc0 | galberding | enum states : int8_t{
|
59 | 27d4e1fa | galberding | 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 | 10bf9cc0 | galberding | 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 | 181f2892 | galberding | }; |
82 | |||
83 | 27d4e1fa | galberding | struct ut_counter{
|
84 | int whiteCount = 0; |
||
85 | 10bf9cc0 | galberding | int ringProxCount = 0; |
86 | fbcb25cc | galberding | // int correctionCount = 0;
|
87 | 27d4e1fa | galberding | int errorCount = 0; |
88 | 10bf9cc0 | galberding | int stateCount = 0; |
89 | fbcb25cc | galberding | int stepCount = 0; |
90 | 10bf9cc0 | galberding | 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 | 27d4e1fa | galberding | }; |
102 | 181f2892 | galberding | |
103 | 10bf9cc0 | galberding | |
104 | struct bottom_prox_calibration {
|
||
105 | bool calibrateBlack = true; |
||
106 | uint32_t buf = 0;
|
||
107 | uint8_t meanWindow = 150;
|
||
108 | }; |
||
109 | |||
110 | 27d4e1fa | galberding | // static const struct ut_counter emptyUtCount;
|
111 | ut_counter utCount; |
||
112 | 10bf9cc0 | galberding | proxy_ctrl pCtrl; |
113 | bottom_prox_calibration proxCalib; |
||
114 | 58fe0e0b | Thomas Schöpping | explicit UserThread();
|
115 | |||
116 | virtual ~UserThread();
|
||
117 | |||
118 | virtual msg_t main();
|
||
119 | 181f2892 | galberding | |
120 | private:
|
||
121 | void setRpmSpeed(const int (&rpmSpeed)[2]); |
||
122 | c9fa414d | galberding | void setRpmSpeedFuzzy(const int (&rpmSpeed)[2]); |
123 | 181f2892 | galberding | 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 | c9fa414d | galberding | int getProxyRingSum();
|
133 | 10bf9cc0 | galberding | |
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 | fbcb25cc | galberding | |
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 | c9fa414d | galberding | |
154 | 181f2892 | galberding | /**
|
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 | 10bf9cc0 | galberding | |
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 | 58fe0e0b | Thomas Schöpping | }; |
176 | |||
177 | } // end of namespace amiro
|
||
178 | |||
179 | #endif // AMIRO_USERTHREAD_H_ |