Revision af93a91c devices/DiWheelDrive/main.cpp
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 |
|
Also available in: Unified diff