Revision 2330e415 devices/DiWheelDrive/linefollow2.cpp

View differences:

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