Revision 84b4c632 devices/DiWheelDrive/main.cpp
devices/DiWheelDrive/main.cpp | ||
---|---|---|
812 | 812 |
chprintf(chp, "Power over Pins: %s Pins Enabled: %s, Power: %d\n", global.ltc4412.isPluggedIn() ? "y" : "n", global.ltc4412.isEnabled() ? "y" : "n", global.robot.getPowerStatus().state_of_charge); |
813 | 813 |
} |
814 | 814 |
|
815 |
#define window 5 |
|
816 |
int32_t counter = 0; |
|
817 |
int32_t proxbuf[window]= { 0 }; |
|
818 |
int32_t meanDeviation(uint16_t a, uint16_t b){ |
|
819 |
int32_t diff = a-b; |
|
820 |
int32_t res = 0; |
|
821 |
proxbuf[counter] = (diff*100)/((a+b)/2); |
|
822 |
for (int i = 0; i< window; i++){ |
|
823 |
res += proxbuf[i]; |
|
824 |
} |
|
825 |
counter++; |
|
826 |
counter = counter % window; |
|
827 |
return res / window; |
|
828 |
} |
|
829 |
|
|
815 | 830 |
|
816 | 831 |
void shellRequestProxyRingValues(BaseSequentialStream *chp, int argc, char *argv[]){ |
817 | 832 |
int steps = 100; |
818 | 833 |
int i; |
819 |
|
|
834 |
int16_t old3 = 0; |
|
835 |
int16_t old4 = 0; |
|
820 | 836 |
uint16_t prox[8]; |
821 | 837 |
uint32_t prox_sum = 0; |
822 | 838 |
|
... | ... | |
828 | 844 |
} |
829 | 845 |
for (int j=0; j<steps; j++){ |
830 | 846 |
prox_sum = 0; |
847 |
old3 = prox[3]; |
|
848 |
old4 = prox[4]; |
|
831 | 849 |
for(i=0; i<8;i++){ |
832 | 850 |
prox[i] = global.robot.getProximityRingValue(i); |
833 | 851 |
prox_sum += prox[i]; |
834 | 852 |
} |
853 |
int32_t deviation = meanDeviation(prox[3] & 0xFFF0 , prox[4] & 0xFFF0); |
|
854 |
// int32_t deviation = meanDeviation((prox[3]+old3) / 2 , (prox[4]+old4) / 2); |
|
855 |
// uint16_t notouch = 100; |
|
856 |
// uint16_t toucht = 20031; |
|
857 |
// sign = |
|
858 |
// i = 0; |
|
859 |
chprintf(chp, "0:%i 1:%i 2:%i 3:%i 4:%i 5:%i 6:%i 7:%i Deviation:%i \n", prox[0], prox[1], prox[2], prox[3], prox[4], prox[5], prox[6], prox[7], deviation); |
|
860 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
861 |
} |
|
862 |
} |
|
863 |
int buf[10][3] = { 0 }; |
|
864 |
void shellRequestMagnetoMeter(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
865 |
int steps = 10; |
|
866 |
// int i; |
|
867 |
|
|
868 |
// uint16_t prox[8]; |
|
869 |
// uint32_t prox_sum = 0; |
|
870 |
|
|
871 |
if (argc == 1){ |
|
872 |
chprintf(chp, "%i steps \n", atoi(argv[0])); |
|
873 |
steps = atoi(argv[0]); |
|
874 |
}else{ |
|
875 |
chprintf(chp, "Usage: proxyRing <steps> \n"); |
|
876 |
} |
|
877 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "motor calibration starts in five seconds...\n"); |
|
878 |
BaseThread::sleep(MS2ST(5000)); |
|
879 |
for (int j=0; j<steps; j++){ |
|
880 |
// prox_sum = 0; |
|
881 |
// for(i=0; i<8;i++){ |
|
882 |
// prox[i] = global.robot.getProximityRingValue(i); |
|
883 |
// prox_sum += prox[i]; |
|
884 |
// } |
|
835 | 885 |
// uint16_t notouch = 100; |
836 | 886 |
// uint16_t toucht = 20031; |
837 | 887 |
// sign = |
838 | 888 |
// i = 0; |
839 |
chprintf(chp, "0:%i 1:%i 2:%i 3:%i 4:%i 5:%i 6:%i 7:%i prox_sum:%i \n", prox[0], prox[1], prox[2], prox[3], prox[4], prox[5], prox[6], prox[7], prox_sum); |
|
889 |
|
|
890 |
if (j < 10){ |
|
891 |
buf[j][0] = global.hmc5883l.getMagnetizationGauss(0x00u); |
|
892 |
buf[j][1] = global.hmc5883l.getMagnetizationGauss(0x02u); |
|
893 |
buf[j][2] = global.hmc5883l.getMagnetizationGauss(0x01u); |
|
894 |
} |
|
895 |
chprintf(chp, "X:%i Y:%i Z:%i\n", global.hmc5883l.getMagnetizationGauss(0x00u), global.hmc5883l.getMagnetizationGauss(0x02u), global.hmc5883l.getMagnetizationGauss(0x01u)); |
|
840 | 896 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
841 | 897 |
} |
842 | 898 |
} |
899 |
void shellRequestMagnetoMeterPrint(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
900 |
for (int j=0; j<10; j++){ |
|
901 |
chprintf(chp, "X:%i Y:%i Z:%i\n", buf[j][0], buf[j][1], buf[j][2]); |
|
902 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
|
903 |
} |
|
904 |
} |
|
905 |
|
|
906 |
void shellRequestPrintCoordinate(BaseSequentialStream *chp, int argc, char *argv[]){ |
|
907 |
types::position oldPos = global.odometry.getPosition(); |
|
908 |
chprintf(chp, "X:%i Y:%i\n",oldPos.x, oldPos.y); |
|
909 |
} |
|
843 | 910 |
|
844 | 911 |
static const ShellCommand commands[] = { |
845 | 912 |
{"shutdown", shellRequestShutdown}, |
... | ... | |
870 | 937 |
{"calibrate_line_sensors", shellRequestCalibrateLineSensores}, |
871 | 938 |
{"printProxyBottom", sellRequestgetBottomSensorData}, |
872 | 939 |
{"printProxyRing", shellRequestProxyRingValues}, |
940 |
{"printMagnetometer", shellRequestMagnetoMeter}, |
|
941 |
{"printMagnetometerRes", shellRequestMagnetoMeterPrint}, |
|
942 |
{"printLocation", shellRequestPrintCoordinate}, |
|
873 | 943 |
{"checkPowerPins", shellRequestCheckPower}, |
874 | 944 |
{NULL, NULL} |
875 | 945 |
}; |
Also available in: Unified diff