Revision 88afb834 devices/DiWheelDrive/linefollow2.cpp

View differences:

devices/DiWheelDrive/linefollow2.cpp
19 19 
    delta = abs(abs(global->threshProxyL-global->threshProxyR) - abs(FL-FR));
20 20 

  		




  21
  21
  
    
    if (FR > global->threshProxyR && FL > global->threshProxyL ){


  		




  22
  
  
    
        return delta * -1;


  		




  
  22
  
    
        return delta ;


  		




  23
  23
  
    
    }else {


  		




  24
  
  
    
        return delta;


  		




  
  24
  
    
        return delta* -1;


  		




  25
  25
  
    
    }


  		




  26
  26
  
    
    return  delta;


  		




  27
  27
  
    
}


  		




  ......




  53
  53
  
    

  		




  54
  54
  
    
}


  		




  55
  55
  
    

  		




  56
  
  
    

  		




  
  56
  
    
int error(){


  		




  
  57
  
    
    int FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();


  		




  
  58
  
    
    int FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();


  		




  
  59
  
    
    int targetL = global->threshProxyL;


  		




  
  60
  
    
    int targetR = global->threshProxyR;


  		




  
  61
  
    
    return FL -targetL + FR - targetR;


  		




  
  62
  
    
}


  		




  57
  63
  
    

  		




  58
  64
  
    
void LineFollow::calibrateZiegler(float KCrit, int rpmSpeed[2]){


  		




  59
  65
  
    
    int targetSpeedL = 5;


  		




  60
  66
  
    
    int targetSpeedR = 5;


  		




  61
  
  
    

  		




  62
  
  
    
    int correctionSpeed = (int) (KCrit * delta());   


  		




  63
  
  
    
    


  		




  64
  
  
    
    rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = targetSpeedL + correctionSpeed;


  		




  65
  
  
    
    rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = targetSpeedR - correctionSpeed;


  		




  66
  
  
    
    chprintf((BaseSequentialStream*) &SD1, "CS:%d,LW:%d,RW:%d\n", correctionSpeed, rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]);


  		




  
  67
  
    
    int delta_ = error();


  		




  
  68
  
    
    int correctionSpeed = (int) (KCrit * delta_);   


  		




  
  69
  
    
    // global->senseRec[global->sensSamples].FL = FL;


  		




  
  70
  
    
    // global->senseRec[global->sensSamples].FR = FR;


  		




  
  71
  
    
    global->senseRec[global->sensSamples].error = delta_;


  		




  
  72
  
    
    global->senseRec[global->sensSamples].FL = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_LEFT].getProximityScaledWoOffset();;


  		




  
  73
  
    
    global->senseRec[global->sensSamples].FR = global->vcnl4020[constants::DiWheelDrive::PROX_FRONT_RIGHT].getProximityScaledWoOffset();;


  		




  
  74
  
    
    global->sensSamples++


  		




  
  75
  
    

  		




  
  76
  
    

  		




  
  77
  
    
    rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   -1*correctionSpeed;


  		




  
  78
  
    
    rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] =  correctionSpeed;


  		




  
  79
  
    
    // chprintf((BaseSequentialStream*) &SD1, "CS:%d,LW:%d,RW:%d\n", correctionSpeed, rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]);


  		




  67
  80
  
    
}


  		












Also available in:  Unified diff