amiro-os / devices / DiWheelDrive / linefollow2.cpp @ 25388c2f
History | View | Annotate | Download (7.69 KB)
1 |
#include "global.hpp" |
---|---|
2 |
#include "linefollow2.hpp" |
3 |
#include <cmath> |
4 |
|
5 |
|
6 |
void LineFollow::printSensorData(){
|
7 |
chprintf((BaseSequentialStream*) &SD1, "Test!");
|
8 |
} |
9 |
|
10 |
// void LineFollow::followLine(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
|
11 |
|
12 |
// chprintf((BaseSequentialStream*) &SD1, "SP: %d,\n", SetPoint);
|
13 |
|
14 |
// // chprintf((BaseSequentialStream*) &SD1, "Proximity: WL:0x%04X FL:0x%04X FR:0x%04X WR:0x%04X\n",
|
15 |
// // vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
|
16 |
// // vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
|
17 |
// // vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
|
18 |
// // vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]);
|
19 |
// // global->motorcontrol.printGains();
|
20 |
// // chprintf((BaseSequentialStream*) &SD1, "Speed -- Left: %d, Right: %d\n", global->motorcontrol.getCurrentRPMLeft(), global->motorcontrol.getCurrentRPMRight());
|
21 |
|
22 |
|
23 |
// // float speedL = global->motorcontrol.getCurrentRPMLeft();
|
24 |
// // float speedR = global->motorcontrol.getCurrentRPMRight();
|
25 |
// // chprintf((BaseSequentialStream*) &SD1, "After motor request SP: %f,\n", SetPoint);
|
26 |
// // Process value
|
27 |
// float processV = static_cast< float >((vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] + vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
|
28 |
// // chprintf((BaseSequentialStream*) &SD1, "PV: %f,\n", processV);
|
29 |
// // chprintf((BaseSequentialStream*) &SD1, "After PV SP: %f,\n", SetPoint);
|
30 |
// float error = SetPoint - processV;
|
31 |
// float d_term = old_error - error;
|
32 |
// // chprintf((BaseSequentialStream*) &SD1, "After Error SP: %f,\n", SetPoint);
|
33 |
// // chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error);
|
34 |
// acc_sum = 0.5 * acc_sum + error;
|
35 |
// int correctionSpeed = static_cast< int >(Kp * error + Ki*acc_sum + Kd*d_term);
|
36 |
// old_error = error;
|
37 |
// chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error);
|
38 |
// chprintf((BaseSequentialStream*) &SD1, "Dterm: %f,\n", d_term);
|
39 |
// chprintf((BaseSequentialStream*) &SD1, "Iterm: %f,\n", acc_sum);
|
40 |
// chprintf((BaseSequentialStream*) &SD1, "New Speed: %d,\n", correctionSpeed);
|
41 |
// // chprintf((BaseSequentialStream*) &SD1, "New Speed: %f, Sum: %f, SP: %f, processV: %f, K_p: %f, K_i: %f \n", correctionSpeed, acc_sum, SetPoint, processV, Kp, Ki);
|
42 |
|
43 |
// // int forward = 15;
|
44 |
// int speedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL] - correctionSpeed;
|
45 |
// int speedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL] + correctionSpeed;
|
46 |
|
47 |
// // if (l_speed )
|
48 |
|
49 |
// rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = speedL;
|
50 |
// rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = speedR;
|
51 |
|
52 |
// chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n", speedL, speedR);
|
53 |
|
54 |
// }
|
55 |
|
56 |
|
57 |
void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){ |
58 |
int targetSensor = 0x38; |
59 |
int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] >> 8; |
60 |
int actualSensorR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] >> 8; |
61 |
int targetSpeedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL];
|
62 |
int targetSpeedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL];
|
63 |
|
64 |
int error = targetSensor - (actualSensorL + actualSensorR);
|
65 |
|
66 |
accSum += error; |
67 |
int dTerm = error - oldError;
|
68 |
|
69 |
int correctionSpeed = (int) (Kp * error + Ki * accSum + Kd * dTerm); |
70 |
chprintf((BaseSequentialStream*) &SD1, "Correction Speed: %d\n", correctionSpeed);
|
71 |
rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correctionSpeed; |
72 |
rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR - correctionSpeed; |
73 |
|
74 |
} |
75 |
|
76 |
|
77 |
// void LineFollow::followLineSeperateSensors2(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
|
78 |
|
79 |
// chprintf((BaseSequentialStream*) &SD1, "SP: %d,\n", SetPoint);
|
80 |
// int targetSensorL = 0x10;
|
81 |
// int targetSensorR = 0x28;
|
82 |
|
83 |
// float actualSpeedL = 20;
|
84 |
// float actualSpeedR = 20;
|
85 |
|
86 |
// // if(actualSpeedL == 0){
|
87 |
// // actualSpeedL = 1;
|
88 |
// // }
|
89 |
// // if(actualSpeedR == 0){
|
90 |
// // actualSpeedR = 1;
|
91 |
// // }
|
92 |
|
93 |
// // Shift sensor values to prevent overflow in following calculation
|
94 |
// int actualSensorL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] >> 8;
|
95 |
// int actualSensorR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] >> 8;
|
96 |
// chprintf((BaseSequentialStream*) &SD1, "Sensor L: %d, R: %d\n", actualSensorL, actualSensorR);
|
97 |
|
98 |
// int targetSpeedL = global->rpmForward[constants::DiWheelDrive::LEFT_WHEEL];
|
99 |
// int targetSpeedR = global->rpmForward[constants::DiWheelDrive::RIGHT_WHEEL];
|
100 |
|
101 |
// int setPointL = targetSensorL;
|
102 |
// int setPointR = targetSensorR;
|
103 |
// chprintf((BaseSequentialStream*) &SD1, "SetPoint L: %d, R: %d\n",setPointL, setPointR );
|
104 |
|
105 |
// int processValueL = actualSensorL;
|
106 |
// int processValueR = actualSensorR;
|
107 |
// chprintf((BaseSequentialStream*) &SD1, "ProcessValue L: %d, R: %d\n",processValueL, processValueR );
|
108 |
|
109 |
// int errorL = setPointL - processValueL;
|
110 |
// int errorR = setPointR - processValueR;
|
111 |
|
112 |
// // This will howfully decrease the overall speed when sensors deviate much
|
113 |
// // errorL /= targetSensorL+actualSensorL;
|
114 |
// // errorR /= targetSensorR+actualSensorR;
|
115 |
// chprintf((BaseSequentialStream*) &SD1, "Error L: %d, R: %d\n",errorL, errorR);
|
116 |
|
117 |
// // int newSpeedL =
|
118 |
// rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = errorL;
|
119 |
// rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = errorR;
|
120 |
|
121 |
// int correction_speedL = (int) (Kp * errorL);
|
122 |
// int correction_speedR = (int) (Kp * errorR);
|
123 |
// chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n",correction_speedL, correction_speedR);
|
124 |
|
125 |
// // // chprintf((BaseSequentialStream*) &SD1, "After motor request SP: %f,\n", SetPoint);
|
126 |
// // // Process value
|
127 |
// // float processV = static_cast< float >((vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] + vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]));
|
128 |
// // // chprintf((BaseSequentialStream*) &SD1, "PV: %f,\n", processV);
|
129 |
// // // chprintf((BaseSequentialStream*) &SD1, "After PV SP: %f,\n", SetPoint);
|
130 |
// // float error = SetPoint - processV;
|
131 |
// // float d_term = old_error - error;
|
132 |
// // // chprintf((BaseSequentialStream*) &SD1, "After Error SP: %f,\n", SetPoint);
|
133 |
// // // chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error);
|
134 |
// // acc_sum = 0.5 * acc_sum + error;
|
135 |
// // int new_speed = static_cast< int >(Kp * error + Ki*acc_sum + Kd*d_term);
|
136 |
// // old_error = error;
|
137 |
// // chprintf((BaseSequentialStream*) &SD1, "Error: %f,\n", error);
|
138 |
// // chprintf((BaseSequentialStream*) &SD1, "Dterm: %f,\n", d_term);
|
139 |
// // chprintf((BaseSequentialStream*) &SD1, "Iterm: %f,\n", acc_sum);
|
140 |
// // chprintf((BaseSequentialStream*) &SD1, "New Speed: %d,\n", new_speed);
|
141 |
// // // chprintf((BaseSequentialStream*) &SD1, "New Speed: %f, Sum: %f, SP: %f, processV: %f, K_p: %f, K_i: %f \n", new_speed, acc_sum, SetPoint, processV, Kp, Ki);
|
142 |
|
143 |
// // // int forward = 15;
|
144 |
// // // int l_speed = forward - new_speed;
|
145 |
// // // int r_speed = forward + new_speed;
|
146 |
|
147 |
// // // if (l_speed )
|
148 |
|
149 |
// rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correction_speedL;
|
150 |
// rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR + correction_speedR;
|
151 |
|
152 |
// // chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n", rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL], rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL]);
|
153 |
|
154 |
// }
|