Revision 88afb834
| devices/DiWheelDrive/global.hpp | ||
|---|---|---|
| 171 | 171 |
int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
|
| 172 | 172 |
|
| 173 | 173 |
// Line Following thresholds set due to calibration |
| 174 |
int threshProxyL = 0; |
|
| 175 |
int threshProxyR = 0; |
|
| 174 |
// MaxDelta: 18676, FL: 4289, FR: 22965 |
|
| 175 |
int threshProxyL = 2168; |
|
| 176 |
int threshProxyR = 24406; |
|
| 177 |
|
|
| 178 |
// Buffer for sensor values |
|
| 179 |
struct sensorRecord |
|
| 180 |
{
|
|
| 181 |
int FL = 0; |
|
| 182 |
int FR = 0; |
|
| 183 |
int delta = 0; |
|
| 184 |
int error = 0; |
|
| 185 |
}; |
|
| 186 |
|
|
| 187 |
int sensSamples = 0; |
|
| 188 |
sensorRecord senseRec[1000]; |
|
| 189 |
|
|
| 176 | 190 |
|
| 177 | 191 |
public: |
| 178 | 192 |
Global() : |
| devices/DiWheelDrive/linefollow2.cpp | ||
|---|---|---|
| 19 | 19 |
delta = abs(abs(global->threshProxyL-global->threshProxyR) - abs(FL-FR)); |
| 20 | 20 |
|
| 21 | 21 |
if (FR > global->threshProxyR && FL > global->threshProxyL ){
|
| 22 |
return delta * -1;
|
|
| 22 |
return delta ; |
|
| 23 | 23 |
}else {
|
| 24 |
return delta; |
|
| 24 |
return delta* -1;
|
|
| 25 | 25 |
} |
| 26 | 26 |
return delta; |
| 27 | 27 |
} |
| ... | ... | |
| 53 | 53 |
|
| 54 | 54 |
} |
| 55 | 55 |
|
| 56 |
|
|
| 56 |
int error(){
|
|
| 57 |
int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
|
| 58 |
int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
| 59 |
int targetL = global->threshProxyL; |
|
| 60 |
int targetR = global->threshProxyR; |
|
| 61 |
return FL -targetL + FR - targetR; |
|
| 62 |
} |
|
| 57 | 63 |
|
| 58 | 64 |
void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){
|
| 59 | 65 |
int targetSpeedL = 5; |
| 60 | 66 |
int targetSpeedR = 5; |
| 61 |
|
|
| 62 |
int correctionSpeed = (int) (KCrit * delta()); |
|
| 63 |
|
|
| 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 |
int delta_ = error(); |
|
| 68 |
int correctionSpeed = (int) (KCrit * delta_); |
|
| 69 |
// global->senseRec[global->sensSamples].FL = FL; |
|
| 70 |
// global->senseRec[global->sensSamples].FR = FR; |
|
| 71 |
global->senseRec[global->sensSamples].error = delta_; |
|
| 72 |
global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();; |
|
| 73 |
global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();; |
|
| 74 |
global->sensSamples++ |
|
| 75 |
|
|
| 76 |
|
|
| 77 |
rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -1*correctionSpeed; |
|
| 78 |
rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = correctionSpeed; |
|
| 79 |
// chprintf((BaseSequentialStream*) &SD1, "CS:%d,LW:%d,RW:%d\n", correctionSpeed, rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]); |
|
| 67 | 80 |
} |
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 754 | 754 |
// sleep(CAN::UPDATE_PERIOD); |
| 755 | 755 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
| 756 | 756 |
} |
| 757 |
chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: 0x%x, FR: 0x%d\n", maxDelta, proxyL, proxyR);
|
|
| 757 |
chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: %d, FR: %d\n", maxDelta, proxyL, proxyR);
|
|
| 758 | 758 |
|
| 759 | 759 |
global.threshProxyL = proxyL; |
| 760 | 760 |
global.threshProxyR = proxyR; |
| ... | ... | |
| 821 | 821 |
int proxyR = global.threshProxyR; |
| 822 | 822 |
int maxDelta = 0; |
| 823 | 823 |
float KCrit = 0.0f; |
| 824 |
global.sensSamples = 0; |
|
| 824 | 825 |
LineFollow lf(&global); |
| 825 | 826 |
|
| 826 | 827 |
if (argc == 2){
|
| ... | ... | |
| 832 | 833 |
chprintf(chp, "Usage: dev_ziegler2 <K_crit> <steps>"); |
| 833 | 834 |
return; |
| 834 | 835 |
} |
| 836 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n"); |
|
| 837 |
BaseThread::sleep(MS2ST(5000)); |
|
| 835 | 838 |
// global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000); |
| 836 | 839 |
|
| 837 | 840 |
for(int s=0; s < steps; s++){
|
| ... | ... | |
| 845 | 848 |
} |
| 846 | 849 |
|
| 847 | 850 |
|
| 851 |
|
|
| 852 |
void recordMove(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 853 |
// int vcnl4020AmbientLight[4]; |
|
| 854 |
int vcnl4020Proximity[4]; |
|
| 855 |
int rpmSpeed[2] = {0};
|
|
| 856 |
int steps = 0; |
|
| 857 |
int speed = 0; |
|
| 858 |
if (argc == 1){
|
|
| 859 |
chprintf(chp, "%i steps \n", atoi(argv[0])); |
|
| 860 |
steps = atoi(argv[0]); |
|
| 861 |
speed = 30; |
|
| 862 |
}else if (argc == 2){
|
|
| 863 |
steps = atoi(argv[0]); |
|
| 864 |
speed = atoi(argv[1]); |
|
| 865 |
}else{
|
|
| 866 |
chprintf(chp, "No steps given!\n"); |
|
| 867 |
return; |
|
| 868 |
} |
|
| 869 |
global.sensSamples = steps; |
|
| 870 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "Recodring starts in five seconds...\n"); |
|
| 871 |
BaseThread::sleep(MS2ST(5000)); |
|
| 872 |
// int sensSamples = 0; |
|
| 873 |
// sensorRecord senseRec[1000]; |
|
| 874 |
|
|
| 875 |
for (int j = 0; j < steps; j++) {
|
|
| 876 |
for (int i = 0; i < 4; i++) {
|
|
| 877 |
// vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
|
| 878 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
|
| 879 |
} |
|
| 880 |
|
|
| 881 |
int FL = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset(); |
|
| 882 |
int FR = global.vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset(); |
|
| 883 |
|
|
| 884 |
|
|
| 885 |
global.senseRec[j].FL = FL; |
|
| 886 |
global.senseRec[j].FR = FR; |
|
| 887 |
global.senseRec[j].delta = delta; |
|
| 888 |
// chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", |
|
| 889 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
|
| 890 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
|
| 891 |
// vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
|
| 892 |
global.motorcontrol.setTargetRPM(speed * 1000000, -speed * 1000000); |
|
| 893 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
| 894 |
} |
|
| 895 |
global.motorcontrol.setTargetRPM(0,0); |
|
| 896 |
for(int k=0; k<8;k++){
|
|
| 897 |
global.robot.setLightColor(k, Color(Color::WHITE)); |
|
| 898 |
} |
|
| 899 |
BaseThread::sleep(MS2ST(1000)); |
|
| 900 |
for(int k=0; k<8;k++){
|
|
| 901 |
global.robot.setLightColor(k, Color(Color::BLACK)); |
|
| 902 |
} |
|
| 903 |
} |
|
| 904 |
|
|
| 905 |
void printMove(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 906 |
|
|
| 907 |
for (int j=0; j<global.sensSamples;j++){
|
|
| 908 |
chprintf(chp,"FL:%d,FR:%d,Delta:%d\n",global.senseRec[j].FL, global.senseRec[j].FR, global.senseRec[j].delta); |
|
| 909 |
} |
|
| 910 |
|
|
| 911 |
} |
|
| 912 |
|
|
| 913 |
|
|
| 848 | 914 |
static const ShellCommand commands[] = {
|
| 849 | 915 |
{"shutdown", shellRequestShutdown},
|
| 850 | 916 |
{"wakeup", shellRequestWakeup},
|
| ... | ... | |
| 875 | 941 |
{"dev_ziegler2", zieglerMeth2},
|
| 876 | 942 |
// TODO: Stop user process from execution to finish/force calibration before anything starts |
| 877 | 943 |
{"calibrate_line", calibrateLineSensores},
|
| 944 |
{"record_move_l", recordMove},
|
|
| 945 |
{"print_record", printMove},
|
|
| 878 | 946 |
{NULL, NULL}
|
| 879 | 947 |
}; |
| 880 | 948 |
|
Also available in: Unified diff