amiro-os / devices / DiWheelDrive / userthread.cpp @ 5d138bca
History | View | Annotate | Download (2.186 KB)
1 |
#include "userthread.hpp" |
---|---|
2 |
|
3 |
#include "global.hpp" |
4 |
|
5 |
#include "linefollow2.hpp" |
6 |
|
7 |
using namespace amiro; |
8 |
|
9 |
extern Global global;
|
10 |
|
11 |
enum class states{ |
12 |
FOLLOW_LINE, |
13 |
OCCUPY, |
14 |
RELEASE, |
15 |
CHARGING |
16 |
}; |
17 |
|
18 |
// User thread state:
|
19 |
states utState = states::FOLLOW_LINE; |
20 |
|
21 |
// a buffer for the z-value of the accelerometer
|
22 |
int16_t accel_z; |
23 |
bool running = false; |
24 |
|
25 |
int vcnl4020AmbientLight[4] = {0}; |
26 |
int vcnl4020Proximity[4] = {0}; |
27 |
|
28 |
|
29 |
// Set the speed by the array
|
30 |
void setRpmSpeed(const int (&rpmSpeed)[2]) { |
31 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
32 |
} |
33 |
|
34 |
void lightOneLed(Color color, int idx){ |
35 |
global.robot.setLightColor(idx, Color(color)); |
36 |
} |
37 |
|
38 |
void lightAllLeds(Color color){
|
39 |
int led = 0; |
40 |
for(led=0; led<8; led++){ |
41 |
lightOneLed(color, led); |
42 |
} |
43 |
} |
44 |
|
45 |
|
46 |
|
47 |
UserThread::UserThread() : |
48 |
chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>() |
49 |
{ |
50 |
} |
51 |
|
52 |
UserThread::~UserThread() |
53 |
{ |
54 |
} |
55 |
|
56 |
msg_t |
57 |
UserThread::main() |
58 |
{ |
59 |
/*
|
60 |
* SETUP
|
61 |
*/
|
62 |
int rpmFuzzyCtrl[2] = {0}; |
63 |
for (uint8_t led = 0; led < 8; ++led) { |
64 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
65 |
} |
66 |
running = false;
|
67 |
LineFollow lf(&global); |
68 |
/*
|
69 |
* LOOP
|
70 |
*/
|
71 |
while (!this->shouldTerminate()) |
72 |
{ |
73 |
/*
|
74 |
* read accelerometer z-value
|
75 |
*/
|
76 |
accel_z = global.lis331dlh.getAccelerationForce(LIS331DLH::AXIS_Z); |
77 |
|
78 |
/*
|
79 |
* evaluate the accelerometer
|
80 |
*/
|
81 |
|
82 |
//Features over can: (Line Following)
|
83 |
//SetKP
|
84 |
//SetKI
|
85 |
//SetKd ?
|
86 |
//SetSleepTime
|
87 |
//SeForwardSpeed? or SetMaxSpeed
|
88 |
//DriveOnLeftLine
|
89 |
//DriveOnRightLine
|
90 |
|
91 |
//if accel_z<-900
|
92 |
//send can event (one time)
|
93 |
// Line following is started if amiro roteted
|
94 |
if (accel_z < -900 /*-0.9g*/) { //new: (can.allowLineFollowOnTurn && accel_z < 900) || can.startLineFollow optional feature |
95 |
// set the front LEDs to blue for one second
|
96 |
lightAllLeds(Color::GREEN); |
97 |
this->sleep(1000); |
98 |
lightAllLeds(Color::BLACK); |
99 |
running = !running; |
100 |
} |
101 |
if(running){
|
102 |
|
103 |
} |
104 |
|
105 |
lf.followLine(rpmFuzzyCtrl); |
106 |
setRpmSpeed(rpmFuzzyCtrl); |
107 |
// this->sleep(US2ST(5));
|
108 |
this->sleep(CAN::UPDATE_PERIOD);
|
109 |
} |
110 |
|
111 |
return RDY_OK;
|
112 |
} |