Revision b8b3a9c9 devices/DiWheelDrive/main.cpp
| devices/DiWheelDrive/main.cpp | ||
|---|---|---|
| 1 |
#include <cstdint> |
|
| 1 | 2 |
#define BL_CALLBACK_TABLE_ADDR (0x08000000 + 0x01C0) |
| 2 | 3 |
#define BL_MAGIC_NUMBER ((uint32_t)0xFF669900u) |
| 3 | 4 |
|
| ... | ... | |
| 17 | 18 |
#include <chprintf.h> |
| 18 | 19 |
#include <shell.h> |
| 19 | 20 |
|
| 20 |
#include "linefollow.hpp"
|
|
| 21 |
|
|
| 21 |
#include "linefollow.hpp" |
|
| 22 |
#include "amiro_map.hpp" |
|
| 22 | 23 |
using namespace chibios_rt; |
| 23 | 24 |
|
| 24 | 25 |
Global global; |
| ... | ... | |
| 697 | 698 |
* Calibrate the thresholds for left and right sensor to get the maximum threshold and to |
| 698 | 699 |
* be able to detect the correction direction. |
| 699 | 700 |
* 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. |
| 700 |
*
|
|
| 701 |
* |
|
| 701 | 702 |
* Note: invert the threshs to drive on the other edge. |
| 702 |
*
|
|
| 703 |
* |
|
| 703 | 704 |
* */ |
| 704 | 705 |
void shellRequestCalibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
|
| 705 | 706 |
// int vcnl4020AmbientLight[4]; |
| ... | ... | |
| 710 | 711 |
int maxDelta = 0; |
| 711 | 712 |
int sensorL = 0; |
| 712 | 713 |
int sensorR = 0; |
| 713 |
|
|
| 714 |
|
|
| 714 | 715 |
if (argc == 1){
|
| 715 | 716 |
chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
| 716 | 717 |
rounds = atoi(argv[0]); |
| 717 |
|
|
| 718 |
|
|
| 718 | 719 |
}else{
|
| 719 | 720 |
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"); |
| 720 | 721 |
return; |
| ... | ... | |
| 748 | 749 |
// delta *= -1; |
| 749 | 750 |
// } |
| 750 | 751 |
|
| 751 |
chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n",
|
|
| 752 |
chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", |
|
| 752 | 753 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
| 753 | 754 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
| 754 | 755 |
delta, |
| ... | ... | |
| 758 | 759 |
// sleep(CAN::UPDATE_PERIOD); |
| 759 | 760 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
| 760 | 761 |
} |
| 761 |
|
|
| 762 |
|
|
| 762 | 763 |
|
| 763 | 764 |
global.linePID.threshProxyL = sensorL / rounds; |
| 764 | 765 |
global.linePID.threshProxyR = sensorR / rounds; |
| ... | ... | |
| 776 | 777 |
if (argc == 1){
|
| 777 | 778 |
chprintf(chp, "Test %i rounds \n", atoi(argv[0])); |
| 778 | 779 |
rounds = atoi(argv[0]); |
| 779 |
|
|
| 780 |
|
|
| 780 | 781 |
} else {
|
| 781 | 782 |
chprintf(chp, "Usage: dev_proxi_sensor_data <rounds> \n"); |
| 782 | 783 |
} |
| ... | ... | |
| 787 | 788 |
// vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight(); |
| 788 | 789 |
vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset(); |
| 789 | 790 |
} |
| 790 |
|
|
| 791 |
|
|
| 791 | 792 |
int32_t delta = (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] |
| 792 | 793 |
- vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]); |
| 793 | 794 |
|
| 794 |
chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n",
|
|
| 795 |
chprintf(chp,"WL:%d,FL:%d,FR:%d,WR:%d,Delta:%d\n", |
|
| 795 | 796 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT], |
| 796 | 797 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT], |
| 797 | 798 |
vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT], |
| ... | ... | |
| 817 | 818 |
int32_t proxbuf[window]= { 0 };
|
| 818 | 819 |
int32_t meanDeviation(uint16_t a, uint16_t b){
|
| 819 | 820 |
int32_t diff = a-b; |
| 820 |
int32_t res = 0;
|
|
| 821 |
int32_t res = 0; |
|
| 821 | 822 |
proxbuf[counter] = (diff*100)/((a+b)/2); |
| 822 | 823 |
for (int i = 0; i< window; i++){
|
| 823 | 824 |
res += proxbuf[i]; |
| ... | ... | |
| 854 | 855 |
// int32_t deviation = meanDeviation((prox[3]+old3) / 2 , (prox[4]+old4) / 2); |
| 855 | 856 |
// uint16_t notouch = 100; |
| 856 | 857 |
// uint16_t toucht = 20031; |
| 857 |
// sign =
|
|
| 858 |
// sign = |
|
| 858 | 859 |
// i = 0; |
| 859 | 860 |
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 | 861 |
BaseThread::sleep(CAN::UPDATE_PERIOD); |
| ... | ... | |
| 884 | 885 |
// } |
| 885 | 886 |
// uint16_t notouch = 100; |
| 886 | 887 |
// uint16_t toucht = 20031; |
| 887 |
// sign =
|
|
| 888 |
// sign = |
|
| 888 | 889 |
// i = 0; |
| 889 | 890 |
|
| 890 | 891 |
if (j < 10){
|
| ... | ... | |
| 964 | 965 |
for (int i=0; i<24;i++){
|
| 965 | 966 |
global.stateTracker[i] = 0; |
| 966 | 967 |
} |
| 968 |
} |
|
| 967 | 969 |
|
| 970 |
inline void setAttributes(uint8_t (&map)[MAX_NODES][NODE_ATTRIBUTES], uint8_t id, |
|
| 971 |
uint8_t l, uint8_t r, uint8_t att) {
|
|
| 972 |
map[id][0] = l; |
|
| 973 |
map[id][1] = r; |
|
| 974 |
map[id][2] = att; |
|
| 975 |
} |
|
| 976 |
|
|
| 977 |
#define TEST_CASES 10 |
|
| 978 |
void shellRequestTestSuite(BaseSequentialStream *chp, int argc, char *argv[]){
|
|
| 979 |
uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES] = {0};
|
|
| 980 |
bool testres[TEST_CASES]; |
|
| 981 |
setAttributes(testmap, 0, 1, 2, 1); |
|
| 982 |
setAttributes(testmap, 1, 2, 2, 0); |
|
| 983 |
setAttributes(testmap, 2, 1, 0, 0); |
|
| 984 |
setAttributes(testmap, 3, 0, 0, 0xff); |
|
| 985 |
AmiroMap map = AmiroMap(&global); |
|
| 986 |
|
|
| 987 |
// -------------------------------------------------- |
|
| 988 |
|
|
| 989 |
int tcase = 0; |
|
| 990 |
map.initialize(testmap); |
|
| 991 |
testres[tcase] = map.get_valid(); |
|
| 992 |
|
|
| 993 |
tcase++; // 1 |
|
| 994 |
setAttributes(testmap, 0, 1, 2, 0xff); |
|
| 995 |
map.initialize(testmap); |
|
| 996 |
testres[tcase] = !map.get_valid(); |
|
| 997 |
|
|
| 998 |
tcase++; // 2 |
|
| 999 |
setAttributes(testmap, 0, 1, 2, 0); |
|
| 1000 |
setAttributes(testmap, 2, 1, 0, 1); |
|
| 1001 |
map.initialize(testmap); |
|
| 1002 |
testres[tcase] = map.get_current() == 2; |
|
| 1003 |
|
|
| 1004 |
// -------------------------------------------------- |
|
| 1005 |
|
|
| 1006 |
int failed = 0; |
|
| 1007 |
int passed = 0; |
|
| 1008 |
for (int i=0; i<=tcase; i++) {
|
|
| 1009 |
if (testres[i]){
|
|
| 1010 |
passed++; |
|
| 1011 |
}else{
|
|
| 1012 |
failed++; |
|
| 1013 |
chprintf(chp, "Test %d Failed\n", i); |
|
| 1014 |
} |
|
| 1015 |
} |
|
| 1016 |
chprintf(chp, "Total: %d, Passed: %d, Failed: %d\n", tcase + 1, passed, failed); |
|
| 1017 |
} |
|
| 1018 |
|
|
| 1019 |
void shellRequestMapTest(BaseSequentialStream *chp, int argc, char *argv[]) {
|
|
| 1020 |
|
|
| 1021 |
// TODO: |
|
| 1022 |
// For now see everything fixed. Create array with pointer to all node structs. Determine |
|
| 1023 |
|
|
| 1024 |
// chprintf(chp, " +-------------------+\n"); |
|
| 1025 |
// chprintf(chp, " | |\n"); |
|
| 1026 |
// chprintf(chp, " | |\n"); |
|
| 1027 |
// chprintf(chp, "+-v--+ |\n"); |
|
| 1028 |
// chprintf(chp, "| 0 | +-++\n"); |
|
| 1029 |
// chprintf(chp, "+-+--+ +--------+2 | <-------+\n"); |
|
| 1030 |
// chprintf(chp, " | | +--+ |\n"); |
|
| 1031 |
// chprintf(chp, " | | ^ |\n"); |
|
| 1032 |
// chprintf(chp, " | | | |\n"); |
|
| 1033 |
// chprintf(chp, " | v | |\n"); |
|
| 1034 |
// chprintf(chp, " | +-+-+------+ |\n"); |
|
| 1035 |
// chprintf(chp, " +------> 1 | |\n"); |
|
| 1036 |
// chprintf(chp, " +---+-------------------+\n"); |
|
| 1037 |
|
|
| 1038 |
uint8_t testmap[MAX_NODES][NODE_ATTRIBUTES] = {0};
|
|
| 1039 |
setAttributes(testmap, 0, 1, 2, 1); |
|
| 1040 |
setAttributes(testmap, 1, 2, 2, 0); |
|
| 1041 |
setAttributes(testmap, 2, 1, 0, 0); |
|
| 1042 |
setAttributes(testmap, 3, 0, 0, 0xff); |
|
| 1043 |
AmiroMap map = AmiroMap(&global); |
|
| 1044 |
map.initialize(testmap); |
|
| 968 | 1045 |
} |
| 969 | 1046 |
|
| 970 | 1047 |
static const ShellCommand commands[] = {
|
| 971 |
{"shutdown", shellRequestShutdown},
|
|
| 972 |
{"wakeup", shellRequestWakeup},
|
|
| 973 |
{"check", shellRequestCheck},
|
|
| 974 |
{"reset_memory", shellRequestResetMemory},
|
|
| 975 |
{"get_board_id", shellRequestGetBoardId},
|
|
| 976 |
{"set_board_id", shellRequestSetBoardId},
|
|
| 977 |
{"get_memory_data", shellRequestGetMemoryData},
|
|
| 978 |
{"get_vcnl", shellRequestGetVcnl},
|
|
| 979 |
{"calib_vcnl_offset", shellRequestCalib},
|
|
| 980 |
{"set_vcnl_offset", shellRequestSetVcnlOffset},
|
|
| 981 |
{"reset_vcnl_offset", shellRequestResetVcnlOffset},
|
|
| 982 |
{"get_vcnl_offset", shellRequestGetVcnlOffset},
|
|
| 983 |
{"reset_Ed_Eb", shellRequestResetCalibrationConstants},
|
|
| 984 |
{"get_Ed_Eb", shellRequestGetCalibrationConstants},
|
|
| 985 |
{"set_Ed_Eb", shellRequestSetCalibrationConstants},
|
|
| 986 |
{"get_robot_id", shellRequestGetRobotId},
|
|
| 987 |
{"get_system_load", shellRequestGetSystemLoad},
|
|
| 988 |
{"set_lights", shellRequestSetLights},
|
|
| 989 |
{"shell_board", shellSwitchBoardCmd},
|
|
| 990 |
{"get_bootloader_info", shellRequestGetBootloaderInfo},
|
|
| 991 |
{"motor_drive", shellRequestMotorDrive},
|
|
| 992 |
{"motor_stop", shellRequestMotorStop},
|
|
| 993 |
{"motor_calibrate", shellRequestMotorCalibrate},
|
|
| 994 |
{"motor_getGains", shellRequestMotorGetGains},
|
|
| 995 |
{"motor_resetGains", shellRequestMotorResetGains},
|
|
| 996 |
{"calibrate_line_sensors", shellRequestCalibrateLineSensores},
|
|
| 997 |
{"printProxyBottom", sellRequestgetBottomSensorData},
|
|
| 998 |
{"printProxyRing", shellRequestProxyRingValues},
|
|
| 999 |
{"printMagnetometer", shellRequestMagnetoMeter},
|
|
| 1000 |
{"printMagnetometerRes", shellRequestMagnetoMeterPrint},
|
|
| 1001 |
{"printLocation", shellRequestPrintCoordinate},
|
|
| 1002 |
{"checkPowerPins", shellRequestCheckPower},
|
|
| 1003 |
{"infos", shellRequestErrorInfo},
|
|
| 1004 |
{NULL, NULL}
|
|
| 1005 |
}; |
|
| 1048 |
{"shutdown", shellRequestShutdown},
|
|
| 1049 |
{"wakeup", shellRequestWakeup},
|
|
| 1050 |
{"check", shellRequestCheck},
|
|
| 1051 |
{"reset_memory", shellRequestResetMemory},
|
|
| 1052 |
{"get_board_id", shellRequestGetBoardId},
|
|
| 1053 |
{"set_board_id", shellRequestSetBoardId},
|
|
| 1054 |
{"get_memory_data", shellRequestGetMemoryData},
|
|
| 1055 |
{"get_vcnl", shellRequestGetVcnl},
|
|
| 1056 |
{"calib_vcnl_offset", shellRequestCalib},
|
|
| 1057 |
{"set_vcnl_offset", shellRequestSetVcnlOffset},
|
|
| 1058 |
{"reset_vcnl_offset", shellRequestResetVcnlOffset},
|
|
| 1059 |
{"get_vcnl_offset", shellRequestGetVcnlOffset},
|
|
| 1060 |
{"reset_Ed_Eb", shellRequestResetCalibrationConstants},
|
|
| 1061 |
{"get_Ed_Eb", shellRequestGetCalibrationConstants},
|
|
| 1062 |
{"set_Ed_Eb", shellRequestSetCalibrationConstants},
|
|
| 1063 |
{"get_robot_id", shellRequestGetRobotId},
|
|
| 1064 |
{"get_system_load", shellRequestGetSystemLoad},
|
|
| 1065 |
{"set_lights", shellRequestSetLights},
|
|
| 1066 |
{"shell_board", shellSwitchBoardCmd},
|
|
| 1067 |
{"get_bootloader_info", shellRequestGetBootloaderInfo},
|
|
| 1068 |
{"motor_drive", shellRequestMotorDrive},
|
|
| 1069 |
{"motor_stop", shellRequestMotorStop},
|
|
| 1070 |
{"motor_calibrate", shellRequestMotorCalibrate},
|
|
| 1071 |
{"motor_getGains", shellRequestMotorGetGains},
|
|
| 1072 |
{"motor_resetGains", shellRequestMotorResetGains},
|
|
| 1073 |
{"calibrate_line_sensors", shellRequestCalibrateLineSensores},
|
|
| 1074 |
{"printProxyBottom", sellRequestgetBottomSensorData},
|
|
| 1075 |
{"printProxyRing", shellRequestProxyRingValues},
|
|
| 1076 |
{"printMagnetometer", shellRequestMagnetoMeter},
|
|
| 1077 |
{"printMagnetometerRes", shellRequestMagnetoMeterPrint},
|
|
| 1078 |
{"printLocation", shellRequestPrintCoordinate},
|
|
| 1079 |
{"checkPowerPins", shellRequestCheckPower},
|
|
| 1080 |
{"stateInfos", shellRequestErrorInfo},
|
|
| 1081 |
{"testMap", shellRequestMapTest},
|
|
| 1082 |
{"test", shellRequestTestSuite },
|
|
| 1083 |
{NULL, NULL}};
|
|
| 1006 | 1084 |
|
| 1007 | 1085 |
static const ShellConfig shell_cfg1 = {
|
| 1008 | 1086 |
(BaseSequentialStream *) &global.sercanmux1, |
Also available in: Unified diff