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