Revision 2330e415 devices/DiWheelDrive/linefollow2.cpp
devices/DiWheelDrive/linefollow2.cpp | ||
---|---|---|
1 | 1 |
#include "global.hpp" |
2 | 2 |
#include "linefollow2.hpp" |
3 |
#include <cmath> |
|
3 | 4 |
|
4 | 5 |
|
5 | 6 |
void LineFollow::printSensorData(){ |
6 | 7 |
chprintf((BaseSequentialStream*) &SD1, "Test!"); |
7 | 8 |
} |
8 | 9 |
|
9 |
void LineFollow::followLine(int vcnl4020Proximity[4], Global *global){ |
|
10 |
chprintf((BaseSequentialStream*) &SD1, "Proximity: WL:0x%04X FL:0x%04X FR:0x%04X WR:0x%04X\n", |
|
11 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
|
12 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
|
13 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
|
14 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT]); |
|
15 |
global->motorcontrol.printGains(); |
|
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 new_speed = 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", new_speed); |
|
41 |
// 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); |
|
42 |
|
|
43 |
// int forward = 15; |
|
44 |
// int l_speed = forward - new_speed; |
|
45 |
// int r_speed = forward + new_speed; |
|
46 |
|
|
47 |
// if (l_speed ) |
|
48 |
|
|
49 |
rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL] = 15 + new_speed; |
|
50 |
rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL] = 15 - new_speed; |
|
51 |
|
|
52 |
chprintf((BaseSequentialStream*) &SD1, "Speed L: %d, R: %d\n", rpmFuzzyCtrl[constants::DiWheelDrive::LEFT_WHEEL], rpmFuzzyCtrl[constants::DiWheelDrive::RIGHT_WHEEL]); |
|
53 |
|
|
16 | 54 |
} |
Also available in: Unified diff