Statistics
| Branch: | Tag: | Revision:

amiro-os / devices / DiWheelDrive / linefollow2.cpp @ 12463563

History | View | Annotate | Download (8.05 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
// }