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