amiro-os / devices / DiWheelDrive / linefollow2.cpp @ af93a91c
History | View | Annotate | Download (2.327 KB)
1 | c76baf23 | Georg Alberding | #include "global.hpp" |
---|---|---|---|
2 | #include "linefollow2.hpp" |
||
3 | 2330e415 | Georg Alberding | #include <cmath> |
4 | c76baf23 | Georg Alberding | |
5 | |||
6 | void LineFollow::printSensorData(){
|
||
7 | chprintf((BaseSequentialStream*) &SD1, "Test!");
|
||
8 | } |
||
9 | |||
10 | 12463563 | galberding | LineFollow::LineFollow(Global *global){ |
11 | this->global = global;
|
||
12 | } |
||
13 | |||
14 | af93a91c | galberding | int LineFollow::delta(){
|
15 | int delta = 0; |
||
16 | int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();
|
||
17 | int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();
|
||
18 | |||
19 | delta = abs(abs(global->threshProxyL-global->threshProxyR) - abs(FL-FR)); |
||
20 | |||
21 | if (FR > global->threshProxyR && FL > global->threshProxyL ){
|
||
22 | return delta * -1; |
||
23 | }else {
|
||
24 | return delta;
|
||
25 | } |
||
26 | return delta;
|
||
27 | } |
||
28 | 25388c2f | Georg Alberding | |
29 | |||
30 | void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){ |
||
31 | int targetSensor = 0x38; |
||
32 | b8085493 | Georg Alberding | int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] ;
|
33 | int actualSensorR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] ;
|
||
34 | 25388c2f | Georg Alberding | int targetSpeedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL];
|
35 | int targetSpeedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL];
|
||
36 | |||
37 | b8085493 | Georg Alberding | |
38 | int diff = actualSensorR - actualSensorL;
|
||
39 | 25388c2f | Georg Alberding | int error = targetSensor - (actualSensorL + actualSensorR);
|
40 | |||
41 | accSum += error; |
||
42 | int dTerm = error - oldError;
|
||
43 | |||
44 | b8085493 | Georg Alberding | if (diff > biggestDiff){
|
45 | biggestDiff = diff; |
||
46 | } |
||
47 | 25388c2f | Georg Alberding | int correctionSpeed = (int) (Kp * error + Ki * accSum + Kd * dTerm); |
48 | chprintf((BaseSequentialStream*) &SD1, "Correction Speed: %d\n", correctionSpeed);
|
||
49 | rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correctionSpeed; |
||
50 | rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR - correctionSpeed; |
||
51 | 2330e415 | Georg Alberding | |
52 | b8085493 | Georg Alberding | chprintf((BaseSequentialStream*) &SD1, "Diff: %d, Biggest: %d\n", correctionSpeed, biggestDiff);
|
53 | |||
54 | c76baf23 | Georg Alberding | } |
55 | 25388c2f | Georg Alberding | |
56 | 12463563 | galberding | |
57 | |||
58 | af93a91c | galberding | void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){ |
59 | int targetSpeedL = 5; |
||
60 | int targetSpeedR = 5; |
||
61 | 25388c2f | Georg Alberding | |
62 | af93a91c | galberding | int correctionSpeed = (int) (KCrit * delta()); |
63 | 25388c2f | Georg Alberding | |
64 | af93a91c | galberding | rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correctionSpeed; |
65 | rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR - correctionSpeed; |
||
66 | chprintf((BaseSequentialStream*) &SD1, "CS:%d,LW:%d,RW:%d\n", correctionSpeed, rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]);
|
||
67 | } |