Revision 84b4c632
| devices/DiWheelDrive/DiWheelDrive.cpp | ||
|---|---|---|
| 192 | 192 |
this->transmitMessage(&frame); |
| 193 | 193 |
} |
| 194 | 194 |
|
| 195 |
void DiWheelDrive::transmitState(uint8_t state){
|
|
| 195 |
void DiWheelDrive::transmitState(int8_t state){
|
|
| 196 | 196 |
CANTxFrame frame; |
| 197 | 197 |
frame.SID = 0; |
| 198 | 198 |
this->encodeDeviceId(&frame, CAN::TRANSMIT_LINE_FOLLOW_STATE); |
| devices/DiWheelDrive/DiWheelDrive.h | ||
|---|---|---|
| 31 | 31 |
* @param power 0 to disable, 1 to enable |
| 32 | 32 |
*/ |
| 33 | 33 |
void requestCharging(uint8_t power); |
| 34 |
void transmitState(uint8_t state);
|
|
| 34 |
void transmitState(int8_t state); |
|
| 35 | 35 |
|
| 36 | 36 |
protected: |
| 37 | 37 |
virtual msg_t receiveMessage(CANRxFrame *frame); |
| 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 |
}; |
| devices/DiWheelDrive/userthread.cpp | ||
|---|---|---|
| 210 | 210 |
|
| 211 | 211 |
// Amiro moved, docking was not successful |
| 212 | 212 |
// if ((start.x + stop_.x) || (start.y + stop_.y)){
|
| 213 |
if (abs(start.x - stop_.x) > 0 /* || (start.y + stop_.y) */){
|
|
| 213 |
if (abs(start.x - stop_.x) > 200 /* || (start.y + stop_.y) */){
|
|
| 214 | 214 |
lightAllLeds(Color::RED); |
| 215 | 215 |
// Enable Motor again if docking was not successful |
| 216 | 216 |
global.motorcontrol.setMotorEnable(true); |
| ... | ... | |
| 544 | 544 |
} |
| 545 | 545 |
} |
| 546 | 546 |
|
| 547 |
// if((devCor.currentDeviation <= -10)){
|
|
| 548 |
// rpmSpeed[0] -= 2000000; |
|
| 549 |
// }else if(devCor.currentDeviation >= 10){
|
|
| 550 |
// rpmSpeed[1] -= 2000000; |
|
| 551 |
// } |
|
| 552 |
// setRpmSpeed(rpmSpeed); |
|
| 547 | 553 |
break; |
| 548 | 554 |
// --------------------------------------- |
| 549 | 555 |
case states::DEVIATION_CORRECTION: |
| ... | ... | |
| 561 | 567 |
rpmSpeed[1] = 0; |
| 562 | 568 |
} |
| 563 | 569 |
setRpmSpeed(rpmSpeed); |
| 564 |
}else if ((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION)){
|
|
| 570 |
}else if (((utCount.stateTime >= DEVIATION_CORRECTION_DURATION / 2) && (utCount.stateTime < DEVIATION_CORRECTION_DURATION +10)) ){
|
|
| 565 | 571 |
if(devCor.RCase){
|
| 566 | 572 |
rpmSpeed[0] = DEVIATION_CORRECTION_SPEED; |
| 567 | 573 |
rpmSpeed[1] = 0; |
| ... | ... | |
| 570 | 576 |
rpmSpeed[1] = DEVIATION_CORRECTION_SPEED; |
| 571 | 577 |
} |
| 572 | 578 |
setRpmSpeed(rpmSpeed); |
| 573 |
}else if (utCount.stateTime >= DEVIATION_CORRECTION_DURATION + 10) {
|
|
| 574 |
// Wait to clear the mean window buffer |
|
| 575 |
setRpmSpeed(stop); |
|
| 579 |
if(((devCor.currentDeviation >= -5) && (devCor.currentDeviation <= 5))){
|
|
| 580 |
utCount.stateTime = 0; |
|
| 581 |
newState = states::REVERSE; |
|
| 582 |
setRpmSpeed(stop); |
|
| 583 |
} |
|
| 576 | 584 |
}else{
|
| 577 | 585 |
utCount.stateTime = 0; |
| 578 | 586 |
newState = states::REVERSE; |
| ... | ... | |
| 740 | 748 |
} |
| 741 | 749 |
prevState = currentState; |
| 742 | 750 |
currentState = newState; |
| 743 |
if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){
|
|
| 744 |
utCount.stateCount = 0; |
|
| 745 |
// chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState); |
|
| 746 |
global.robot.transmitState(currentState); |
|
| 747 |
// global.robot.setOdometry(global.odometry.getPosition()); |
|
| 751 |
// if (utCount.stateCount > CAN_TRANSMIT_STATE_THRESH){
|
|
| 752 |
// utCount.stateCount = 0;
|
|
| 753 |
// // chprintf((BaseSequentialStream*)&global.sercanmux1, "Transmit state %d\n", newState);
|
|
| 754 |
// global.robot.transmitState(currentState);
|
|
| 755 |
// // global.robot.setOdometry(global.odometry.getPosition());
|
|
| 748 | 756 |
|
| 749 |
}else{
|
|
| 750 |
utCount.stateCount++; |
|
| 751 |
} |
|
| 757 |
// }else{
|
|
| 758 |
// utCount.stateCount++;
|
|
| 759 |
// }
|
|
| 752 | 760 |
this->sleep(CAN::UPDATE_PERIOD); |
| 753 | 761 |
} |
| 754 | 762 |
|
| devices/DiWheelDrive/userthread.hpp | ||
|---|---|---|
| 47 | 47 |
// Threshold for failing to dock |
| 48 | 48 |
#define DOCKING_ERROR_THRESH 3 |
| 49 | 49 |
#define CAN_TRANSMIT_STATE_THRESH 50 |
| 50 |
#define PROX_DEVIATION_MEAN_WINDOW 5
|
|
| 50 |
#define PROX_DEVIATION_MEAN_WINDOW 3
|
|
| 51 | 51 |
#define MAX_DEVIATION_CORRECTIONS 4 |
| 52 |
#define MAX_DEVIATION_FACTOR 45
|
|
| 53 |
#define DEVIATION_CORRECTION_DURATION 1000
|
|
| 52 |
#define MAX_DEVIATION_FACTOR 35
|
|
| 53 |
#define DEVIATION_CORRECTION_DURATION 900
|
|
| 54 | 54 |
#define DEVIATION_CORRECTION_SPEED 2000000 |
| 55 | 55 |
#define DEVIATION_CORRECTION_VALUE (DEVIATION_CORRECTION_SPEED / 2) |
| 56 |
#define DEVIATION_DIST_THRESH 6000
|
|
| 56 |
#define DEVIATION_DIST_THRESH 25000
|
|
| 57 | 57 |
|
| 58 | 58 |
namespace amiro {
|
| 59 | 59 |
|
Also available in: Unified diff