Revision 84b4c632 devices/DiWheelDrive/main.cpp

View differences:

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