Revision af93a91c
| devices/DiWheelDrive/linefollow2.cpp | ||
|---|---|---|
| 11 | 11 |
this->global = global; |
| 12 | 12 |
} |
| 13 | 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 |
// } |
|
| 14 |
int LineFollow::delta(){
|
|
| 15 |
int delta = 0; |
|
| 16 |
int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
|
| 17 |
int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
| 18 |
|
|
| 19 |
delta = abs(abs(global->threshProxyL-global->threshProxyR) - abs(FL-FR)); |
|
| 20 |
|
|
| 21 |
if (FR > global->threshProxyR && FL > global->threshProxyL ){
|
|
| 22 |
return delta * -1; |
|
| 23 |
}else {
|
|
| 24 |
return delta; |
|
| 25 |
} |
|
| 26 |
return delta; |
|
| 27 |
} |
|
| 59 | 28 |
|
| 60 | 29 |
|
| 61 | 30 |
void LineFollow::stableFollow(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
|
| ... | ... | |
| 84 | 53 |
|
| 85 | 54 |
} |
| 86 | 55 |
|
| 87 |
int calculateError(){
|
|
| 88 |
|
|
| 89 |
} |
|
| 90 | 56 |
|
| 91 |
void calibrateZiegler(int (&rpmFuzzyCtrl)[2], Global *global){
|
|
| 92 | 57 |
|
| 93 |
} |
|
| 58 |
void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){
|
|
| 59 |
int targetSpeedL = 5; |
|
| 60 |
int targetSpeedR = 5; |
|
| 94 | 61 |
|
| 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; |
|
| 62 |
int correctionSpeed = (int) (KCrit * delta()); |
|
| 138 | 63 |
|
| 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 |
// } |
|
| 64 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correctionSpeed; |
|
| 65 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR - correctionSpeed; |
|
| 66 |
chprintf((BaseSequentialStream*) &SD1, "CS:%d,LW:%d,RW:%d\n", correctionSpeed, rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]); |
|
| 67 |
} |
|
| devices/DiWheelDrive/linefollow2.hpp | ||
|---|---|---|
| 22 | 22 |
int biggestDiff = 0; |
| 23 | 23 |
Global *global; |
| 24 | 24 |
LineFollow(Global *global); |
| 25 |
void calibrateZiegler(float KCrit, int rpmSpeed[2]); |
|
| 26 |
|
|
| 27 |
private: |
|
| 28 |
int delta(); |
|
| 25 | 29 |
|
| 26 | 30 |
}; |
| 27 | 31 |
|
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 17 | 17 |
#include <chprintf.h> |
| 18 | 18 |
#include <shell.h> |
| 19 | 19 |
|
| 20 |
#include "linefollow2.hpp" |
|
| 21 |
|
|
| 20 | 22 |
using namespace chibios_rt; |
| 21 | 23 |
|
| 22 | 24 |
Global global; |
| ... | ... | |
| 715 | 717 |
chprintf(chp, "Usage: calbrate_line_sensors [1,n]\nThis will calibrate the thresholds for the left and right sensor\naccording to the maximum delta value recorded.\n"); |
| 716 | 718 |
return; |
| 717 | 719 |
} |
| 718 |
|
|
| 720 |
for (uint8_t led = 0; led < 8; ++led) {
|
|
| 721 |
global.robot.setLightColor(led, Color(Color::BLACK)); |
|
| 722 |
} |
|
| 719 | 723 |
|
| 720 | 724 |
for (int j = 0; j < rounds; j++) {
|
| 721 | 725 |
for (int i = 0; i < 4; i++) {
|
| 722 | 726 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
| 723 | 727 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
| 724 | 728 |
} |
| 725 |
|
|
| 729 |
global.robot.setLightColor(j % 8, Color(Color::BLACK)); |
|
| 730 |
global.robot.setLightColor(j+1 % 8, Color(Color::WHITE)); |
|
| 726 | 731 |
int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] |
| 727 | 732 |
- vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
| 728 | 733 |
// Update proximity thresh |
| 729 | 734 |
if (delta > maxDelta) {
|
| 735 |
for (uint8_t led = 0; led < 8; ++led) {
|
|
| 736 |
global.robot.setLightColor(led, Color(Color::GREEN)); |
|
| 737 |
} |
|
| 730 | 738 |
maxDelta = delta; |
| 731 | 739 |
proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]; |
| 732 | 740 |
proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]; |
| ... | ... | |
| 756 | 764 |
|
| 757 | 765 |
|
| 758 | 766 |
void proxySensorData(BaseSequentialStream *chp, int argc, char *argv[]) {
|
| 759 |
int vcnl4020AmbientLight[4];
|
|
| 760 |
int vcnl4020Proximity[4];
|
|
| 761 |
int rounds = 1;
|
|
| 762 |
int proxyL = global.threshProxyL;
|
|
| 763 |
int proxyR = global.threshProxyR;
|
|
| 764 |
int maxDelta = 0;
|
|
| 767 |
uint16_t vcnl4020AmbientLight[4];
|
|
| 768 |
uint16_t vcnl4020Proximity[4];
|
|
| 769 |
uint16_t rounds = 1;
|
|
| 770 |
uint16_t proxyL = global.threshProxyL;
|
|
| 771 |
uint16_t proxyR = global.threshProxyR;
|
|
| 772 |
uint16_t maxDelta = 0;
|
|
| 765 | 773 |
|
| 766 | 774 |
if (argc == 1){
|
| 767 | 775 |
chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
| 768 | 776 |
rounds = atoi(argv[0]); |
| 769 | 777 |
|
| 770 | 778 |
} |
| 779 |
|
|
| 771 | 780 |
|
| 772 | 781 |
for (int j = 0; j < rounds; j++) {
|
| 773 | 782 |
for (int i = 0; i < 4; i++) {
|
| ... | ... | |
| 775 | 784 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
| 776 | 785 |
} |
| 777 | 786 |
|
| 778 |
int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
|
|
| 787 |
uint16_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
|
|
| 779 | 788 |
- vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
| 780 | 789 |
// // Update proximity thresh |
| 781 | 790 |
// if (delta > maxDelta) {
|
| ... | ... | |
| 784 | 793 |
// proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]; |
| 785 | 794 |
// } |
| 786 | 795 |
|
| 787 |
if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
|
|
| 788 |
delta *= -1; |
|
| 789 |
} |
|
| 796 |
// if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
|
|
| 797 |
// delta *= -1;
|
|
| 798 |
// }
|
|
| 790 | 799 |
|
| 791 |
chprintf(chp,"WL: %x, FL: %x, FR: %x, WR: %x, ProxyL: %x, ProxyR: %x, Delta: %d\n",
|
|
| 800 |
chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n",
|
|
| 792 | 801 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
| 793 | 802 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
| 794 | 803 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
| 795 | 804 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], |
| 796 |
proxyL, |
|
| 797 |
proxyR, |
|
| 798 | 805 |
delta); |
| 799 | 806 |
// sleep(CAN::UPDATE_PERIOD); |
| 800 | 807 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
| 801 | 808 |
} |
| 802 |
chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR); |
|
| 809 |
// chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
|
|
| 803 | 810 |
return; |
| 804 | 811 |
} |
| 805 | 812 |
|
| 806 | 813 |
|
| 807 | 814 |
|
| 815 |
void zieglerMeth2(BaseSequentialStream *chp, int argc, char *argv[]) {
|
|
| 816 |
int vcnl4020AmbientLight[4]; |
|
| 817 |
int vcnl4020Proximity[4]; |
|
| 818 |
int rpmSpeed[2] = {0};
|
|
| 819 |
int steps = 0; |
|
| 820 |
int proxyL = global.threshProxyL; |
|
| 821 |
int proxyR = global.threshProxyR; |
|
| 822 |
int maxDelta = 0; |
|
| 823 |
float KCrit = 0.0f; |
|
| 824 |
LineFollow lf(&global); |
|
| 825 |
|
|
| 826 |
if (argc == 2){
|
|
| 827 |
chprintf(chp, "KCrti %f\n", atof(argv[0])); |
|
| 828 |
chprintf(chp, "Steps %i\n", atoi(argv[1])); |
|
| 829 |
KCrit = atof(argv[0]); |
|
| 830 |
steps = atoi(argv[1]); |
|
| 831 |
} else{
|
|
| 832 |
chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps>"); |
|
| 833 |
return; |
|
| 834 |
} |
|
| 835 |
// global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
| 836 |
|
|
| 837 |
for(int s=0; s < steps; s++){
|
|
| 838 |
chprintf(chp,"S:%d,",s); |
|
| 839 |
lf.calibrateZiegler(KCrit, rpmSpeed); |
|
| 840 |
|
|
| 841 |
global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
|
| 842 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 843 |
} |
|
| 844 |
global.motorcontrol.setTargetRPM(0,0); |
|
| 845 |
} |
|
| 846 |
|
|
| 847 |
|
|
| 808 | 848 |
static const ShellCommand commands[] = {
|
| 809 | 849 |
{"shutdown", shellRequestShutdown},
|
| 810 | 850 |
{"wakeup", shellRequestWakeup},
|
| ... | ... | |
| 832 | 872 |
{"motor_getGains", shellRequestMotorGetGains},
|
| 833 | 873 |
{"motor_resetGains", shellRequestMotorResetGains},
|
| 834 | 874 |
{"dev_proxi_sensor_data", proxySensorData},
|
| 835 |
{"calibrate_line", calibrateLineSensores},
|
|
| 875 |
{"dev_ziegler2", zieglerMeth2},
|
|
| 876 |
// TODO: Stop user process from execution to finish/force calibration before anything starts |
|
| 877 |
{"calibrate_line", calibrateLineSensores},
|
|
| 836 | 878 |
{NULL, NULL}
|
| 837 | 879 |
}; |
| 838 | 880 |
|
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 462 | 462 |
// lineFollownew |
| 463 | 463 |
//else |
| 464 | 464 |
// lineFollowing(vcnl4020Proximity, rpmFuzzyCtrl, &global); |
| 465 |
// setRpmSpeed(rpmFuzzyCtrl);
|
|
| 465 |
setRpmSpeed(rpmFuzzyCtrl); |
|
| 466 | 466 |
} |
| 467 | 467 |
|
| 468 | 468 |
// this->sleep(US2ST(5)); |
Also available in: Unified diff