amiro-os / devices / DiWheelDrive / linefollow2.cpp @ 2330e415
History | View | Annotate | Download (2.74 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 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 |
|
54 |
} |