Revision 12463563

View differences:

devices/DiWheelDrive/global.hpp
170 170
  int rpmSoftRight[2] = {rpmSoftLeft[1],rpmSoftLeft[0]};
171 171
  int rpmHardRight[2] = {rpmHardLeft[1],rpmHardLeft[0]};
172 172

  
173
  // Line Following thresholds set due to calibration
174
  int threshProxyL = 0;
175
  int threshProxyR = 0;
176

  
173 177
public:
174 178
  Global() :
175 179
    HW_I2C1(&I2CD1), HW_I2C2(&I2CD2),
devices/DiWheelDrive/linefollow2.cpp
7 7
    chprintf((BaseSequentialStream*) &SD1, "Test!");
8 8
}
9 9

  
10
LineFollow::LineFollow(Global *global){
11
    this->global = global;
12
}
13

  
10 14
// void LineFollow::followLine(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
11 15
    
12 16
//     chprintf((BaseSequentialStream*) &SD1, "SP: %d,\n", SetPoint);
......
80 84

  
81 85
}
82 86

  
87
int calculateError(){
88
    
89
}
90

  
91
void calibrateZiegler(int (&rpmFuzzyCtrl)[2], Global *global){
92

  
93
}
83 94

  
84 95
// void LineFollow::followLineSeperateSensors2(int vcnl4020Proximity[4], int (&rpmFuzzyCtrl)[2], Global *global){
85 96
    
devices/DiWheelDrive/linefollow2.hpp
20 20
  int accSum = 0;
21 21
  float oldError = 0;
22 22
  int biggestDiff = 0;
23
  Global *global;
24
  LineFollow(Global *global);
23 25

  
24 26
};
25 27

  
devices/DiWheelDrive/main.cpp
690 690
  return;
691 691
}
692 692

  
693

  
694
/**
695
 * Calibrate the thresholds for left and right sensor to get the maximum threshold and to
696
 * be able to detect the correction direction.
697
 * 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.
698
 * 
699
 * Note: invert the threshs to drive on the other edge.
700
 * 
701
 * */
702
void calibrateLineSensores(BaseSequentialStream *chp, int argc, char *argv[]) {
703
    int vcnl4020AmbientLight[4];
704
    int vcnl4020Proximity[4];
705
    int rounds = 1;
706
    int proxyL = 0;
707
    int proxyR = 0;
708
    int maxDelta = 0;
709
 
710
  if (argc == 1){
711
    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
712
    rounds = atoi(argv[0]);
713
    
714
  }else{
715
    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
    return;
717
  }
718

  
719

  
720
  for (int j = 0; j < rounds; j++) {
721
    for (int i = 0; i < 4; i++) {
722
        vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
723
        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
724
    }
725

  
726
    int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
727
                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
728
    // Update proximity thresh
729
    if (delta > maxDelta) {
730
      maxDelta = delta;
731
      proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
732
      proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
733
    }
734

  
735
    // if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
736
    //   delta *= -1;
737
    // }
738

  
739
    chprintf(chp,"FL: 0x%x, FR: 0x%x, Delta: %d, ProxyL: %x, ProxyR: %x, MaxDelta: %d\n", 
740
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
741
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
742
                  delta,
743
                  proxyL,
744
                  proxyR,
745
                  maxDelta);
746
    // sleep(CAN::UPDATE_PERIOD);
747
    BaseThread::sleep(CAN::UPDATE_PERIOD);
748
  }
749
  chprintf(chp,"Sensors Calibrated: MaxDelta: %d, FL: 0x%x, FR: 0x%d\n", maxDelta, proxyL, proxyR);
750

  
751
  global.threshProxyL = proxyL;
752
  global.threshProxyR = proxyR;
753
  return;
754
}
755

  
756

  
757

  
758
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;
765
 
766
  if (argc == 1){
767
    chprintf(chp, "Test %i rounds \n", atoi(argv[0]));
768
    rounds = atoi(argv[0]);
769
    
770
  }
771

  
772
  for (int j = 0; j < rounds; j++) {
773
    for (int i = 0; i < 4; i++) {
774
        vcnl4020AmbientLight[i] = global.vcnl4020[i].getAmbientLight();
775
        vcnl4020Proximity[i] = global.vcnl4020[i].getProximityScaledWoOffset();
776
    }
777

  
778
    int delta = abs(vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT]
779
                  - vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT]);
780
    // // Update proximity thresh
781
    // if (delta > maxDelta) {
782
    //   maxDelta = delta;
783
    //   proxyL = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT];
784
    //   proxyR = vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT];
785
    // }
786

  
787
    if (vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT] > proxyR && vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT] > proxyL ){
788
      delta *= -1;
789
    }
790

  
791
    chprintf(chp,"WL: %x, FL: %x, FR: %x, WR: %x, ProxyL: %x, ProxyR: %x, Delta: %d\n", 
792
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_LEFT],
793
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_LEFT],
794
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_FRONT_RIGHT],
795
                  vcnl4020Proximity[constants::DiWheelDrive::PROX_WHEEL_RIGHT],
796
                  proxyL,
797
                  proxyR,
798
                  delta);
799
    // sleep(CAN::UPDATE_PERIOD);
800
    BaseThread::sleep(CAN::UPDATE_PERIOD);
801
  }
802
  chprintf(chp,"Summary: MaxDelta: %d, FL: %x, FR: %d\n", maxDelta, proxyL, proxyR);
803
  return;
804
}
805

  
806

  
807

  
693 808
static const ShellCommand commands[] = {
694 809
  {"shutdown", shellRequestShutdown},
695 810
  {"wakeup", shellRequestWakeup},
......
716 831
  {"motor_calibrate", shellRequestMotorCalibrate},
717 832
  {"motor_getGains", shellRequestMotorGetGains},
718 833
  {"motor_resetGains", shellRequestMotorResetGains},
834
  {"dev_proxi_sensor_data", proxySensorData},
835
  {"calibrate_line", calibrateLineSensores},
719 836
  {NULL, NULL}
720 837
};
721 838

  
devices/DiWheelDrive/userthread.cpp
394 394
		global.robot.setLightColor(led, Color(Color::BLACK));
395 395
    }
396 396
    running = false;
397
	LineFollow lf;
397
	LineFollow lf(&global);
398 398
	/*
399 399
	 * LOOP
400 400
	 */

Also available in: Unified diff