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