Revision 12463563 devices/DiWheelDrive/main.cpp
devices/DiWheelDrive/main.cpp | ||
---|---|---|
690 | 690 |
return; |
691 | 691 |
} |
692 | 692 |
|
693 |
|
|
694 |
/** |
|
695 |
* Calibrate the thresholds for left and right sensor to get the maximum threshold and to |
|
696 |
* be able to detect the correction direction. |
|
697 |
* In this case it is expected that the FL-Sensor sould be in the white part of the edge and the FR-Sensor in the black one. |
|
698 |
* |
|
699 |
* Note: invert the threshs to drive on the other edge. |
|
700 |
* |
|
701 |
* */ |
|
702 |
void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) { |
|
703 |
int vcnl4020AmbientLight[4]; |
|
704 |
int vcnl4020Proximity[4]; |
|
705 |
int rounds = 1; |
|
706 |
int proxyL = 0; |
|
707 |
int proxyR = 0; |
|
708 |
int maxDelta = 0; |
|
709 |
|
|
710 |
if (argc == 1){ |
|
711 |
chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
|
712 |
rounds = atoi(argv[0]); |
|
713 |
|
|
714 |
}else{ |
|
715 |
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 |
return; |
|
717 |
} |
|
718 |
|
|
719 |
|
|
720 |
for (int j = 0; j < rounds; j++) { |
|
721 |
for (int i = 0; i < 4; i++) { |
|
722 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
|
723 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
|
724 |
} |
|
725 |
|
|
726 |
int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] |
|
727 |
- vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
|
728 |
// Update proximity thresh |
|
729 |
if (delta > maxDelta) { |
|
730 |
maxDelta = delta; |
|
731 |
proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]; |
|
732 |
proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]; |
|
733 |
} |
|
734 |
|
|
735 |
// if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){ |
|
736 |
// delta *= -1; |
|
737 |
// } |
|
738 |
|
|
739 |
chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", |
|
740 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
|
741 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
|
742 |
delta, |
|
743 |
proxyL, |
|
744 |
proxyR, |
|
745 |
maxDelta); |
|
746 |
// sleep(CAN::UPDATE_PERIOD); |
|
747 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
748 |
} |
|
749 |
chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: 0x%x, FR: 0x%d\n", maxDelta, proxyL, proxyR); |
|
750 |
|
|
751 |
global.threshProxyL = proxyL; |
|
752 |
global.threshProxyR = proxyR; |
|
753 |
return; |
|
754 |
} |
|
755 |
|
|
756 |
|
|
757 |
|
|
758 |
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; |
|
765 |
|
|
766 |
if (argc == 1){ |
|
767 |
chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
|
768 |
rounds = atoi(argv[0]); |
|
769 |
|
|
770 |
} |
|
771 |
|
|
772 |
for (int j = 0; j < rounds; j++) { |
|
773 |
for (int i = 0; i < 4; i++) { |
|
774 |
vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
|
775 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
|
776 |
} |
|
777 |
|
|
778 |
int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] |
|
779 |
- vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
|
780 |
// // Update proximity thresh |
|
781 |
// if (delta > maxDelta) { |
|
782 |
// maxDelta = delta; |
|
783 |
// proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]; |
|
784 |
// proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]; |
|
785 |
// } |
|
786 |
|
|
787 |
if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){ |
|
788 |
delta *= -1; |
|
789 |
} |
|
790 |
|
|
791 |
chprintf(chp,"WL: %x, FL: %x, FR: %x, WR: %x, ProxyL: %x, ProxyR: %x, Delta: %d\n", |
|
792 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
|
793 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
|
794 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
|
795 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT], |
|
796 |
proxyL, |
|
797 |
proxyR, |
|
798 |
delta); |
|
799 |
// sleep(CAN::UPDATE_PERIOD); |
|
800 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
801 |
} |
|
802 |
chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR); |
|
803 |
return; |
|
804 |
} |
|
805 |
|
|
806 |
|
|
807 |
|
|
693 | 808 |
static const ShellCommand commands[] = { |
694 | 809 |
{"shutdown", shellRequestShutdown}, |
695 | 810 |
{"wakeup", shellRequestWakeup}, |
... | ... | |
716 | 831 |
{"motor_calibrate", shellRequestMotorCalibrate}, |
717 | 832 |
{"motor_getGains", shellRequestMotorGetGains}, |
718 | 833 |
{"motor_resetGains", shellRequestMotorResetGains}, |
834 |
{"dev_proxi_sensor_data", proxySensorData}, |
|
835 |
{"calibrate_line", calibrateLineSensores}, |
|
719 | 836 |
{NULL, NULL} |
720 | 837 |
}; |
721 | 838 |
|
Also available in: Unified diff