Revision c9fa414d

View differences:

devices/DiWheelDrive/linefollow.cpp
120 120

  
121 121
    case LineFollowStrategy::REVERSE:
122 122
      correctionSpeed = -getPidCorrectionSpeed();
123
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -global->forwardSpeed;
123
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] = -1000000*global->forwardSpeed;
124 124

  
125
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -global->forwardSpeed;
125
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = -1000000*global->forwardSpeed;
126 126
      
127 127
      break;
128 128

  
......
130 130
      correctionSpeed = getPidCorrectionSpeed();
131 131
      // chprintf((BaseSequentialStream*) &SD1, "Correction: %d, thresh: %d\n",correctionSpeed,  global->threshWhite);
132 132

  
133
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   global->forwardSpeed + correctionSpeed;
133
      rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] =   1000000*global->forwardSpeed + correctionSpeed;
134 134

  
135
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = global->forwardSpeed - correctionSpeed;
135
      rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] = 1000000*global->forwardSpeed - correctionSpeed;
136 136
      break;
137 137
    }
138 138
    return whiteFlag;
......
143 143
 * Pid controller which returns a corrections speed.
144 144
 */
145 145
int LineFollow::getPidCorrectionSpeed(){
146
    int error = getError();
147
    int sloap = oldError - error ;
146
    int32_t error = getError();
147
    int32_t sloap = oldError - error ;
148 148
    // int correctionSpeed = (int) (global->K_p*error + Ki*accumHist - global->K_d*sloap);
149
    int correctionSpeed = (int) (K_p*error + K_i*accumHist - K_d*sloap);
149
    int32_t correctionSpeed = (int32_t) (K_p*error + K_i*accumHist + K_d*sloap);
150 150
    oldError = error;
151
    // accumHist += (int) (0.01 * error);
151
    accumHist += error;
152 152
    if (abs(error) > global->maxDist.error){
153 153
        global->maxDist.error = error;
154 154
    }
devices/DiWheelDrive/linefollow.hpp
116 116
  int trans = 0;
117 117

  
118 118
  // PID controller components ---------------
119
  float K_p = 0.002;
120
  float K_i = 0;
121
  float K_d = 0;
122
  int accumHist = 0;
119
  int32_t K_p = 1400;
120
  float K_i = 0.06;
121
  float K_d = 0.2;
122
  int32_t accumHist = 0;
123 123
  float oldError = 0;
124 124
  // ----------------------------------------
125 125
};
devices/DiWheelDrive/userthread.cpp
16 16
 * 
17 17
 * @param rpmSpeed speed for left and right wheel in rounds/min
18 18
 */
19
void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) {
19
void UserThread::setRpmSpeedFuzzy(const int (&rpmSpeed)[2]) {
20 20
  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL] * 1000000, rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL] * 1000000);
21 21
}
22 22

  
23
void UserThread::setRpmSpeed(const int (&rpmSpeed)[2]) {
24
  global.motorcontrol.setTargetRPM(rpmSpeed[constants::DiWheelDrive::LEFT_WHEEL], rpmSpeed[constants::DiWheelDrive::RIGHT_WHEEL]);
25
}
26

  
23 27
void UserThread::lightOneLed(Color color, int idx){
24 28
  global.robot.setLightColor(idx, Color(color));
25 29
}
......
99 103
  return success;
100 104
}
101 105

  
102
uint16_t UserThread::getProxyRingSum(){
103
  uint16_t prox_sum = 0;
106
int UserThread::getProxyRingSum(){
107
  int prox_sum = 0;
104 108
  for(int i=0; i<8;i++){
105 109
    prox_sum += global.robot.getProximityRingValue(i);;
106 110
  }
......
138 142
  int rpmSpeed[2] = {0};
139 143
  int stop[2] = {0};
140 144
  int turn[2] = {5,-5};
141
  LineFollowStrategy lStrategy = LineFollowStrategy::FUZZY;
145
  LineFollowStrategy lStrategy = LineFollowStrategy::EDGE_RIGHT;
142 146
  for (uint8_t led = 0; led < 8; ++led) {
143 147
    global.robot.setLightColor(led, Color(Color::BLACK));
144 148
  }
......
276 280
        // whiteBuf = 0;
277 281
        // proxyBuf = 0;
278 282
        // lf.followLine(rpmSpeed);
279
        setRpmSpeed(rpmSpeed);
283
        if (lf.getStrategy() == LineFollowStrategy::FUZZY){
284
          setRpmSpeedFuzzy(rpmSpeed);
285
        }else{
286

  
287
          setRpmSpeed(rpmSpeed);
288
        }
280 289
        
281 290
        break;
282 291
      // ---------------------------------------
......
373 382
          }else{
374 383
            // No voltage on pins -> falsely docked
375 384
            // deactivate pins
385
            if (!global.motorcontrol.getMotorEnable()){
386
            global.motorcontrol.toggleMotorEnable();
387
            }
376 388
            global.robot.requestCharging(0);
377 389
            if((rProx[0] >= PROX_MAX_VAL) && (rProx[7] >= PROX_MAX_VAL)){
378 390
              newState = states::RELEASE_TO_CORRECT;
devices/DiWheelDrive/userthread.hpp
71 71

  
72 72
private:
73 73
  void setRpmSpeed(const int (&rpmSpeed)[2]);
74
  void setRpmSpeedFuzzy(const int (&rpmSpeed)[2]);
74 75
  void lightOneLed(Color color, int idx);
75 76
  void lightAllLeds(Color color);
76 77
  /**
......
80 81
  void checkForMotion();
81 82
  bool checkPinVoltage();
82 83
  bool checkPinEnabled();
83
  uint16_t getProxyRingSum();
84
  int getProxyRingSum();
85

  
84 86

  
85 87
  /**
86 88
   * Check if current position changes when the wheel are deactivated.

Also available in: Unified diff