()
| 76 | integralArea = 0.0 |
| 77 | |
| 78 | def controller(): |
| 79 | global stopper, gpg, lf, stepSize, loopFreq, setPoint, motorSpeed, leftMotorSpeed, rightMotorSpeed,stopMotors, Kp, Ki, Kd |
| 80 | global integralArea |
| 81 | loopPeriod = 1 / loopFreq |
| 82 | |
| 83 | integralArea = 0.0 |
| 84 | previousError = 0.0 |
| 85 | |
| 86 | try: |
| 87 | while not stopper.is_set(): |
| 88 | start = time() |
| 89 | |
| 90 | # <0.5 when line is on the left |
| 91 | # >0.5 when line is on the right |
| 92 | current, _ = lf.read('weighted-avg') |
| 93 | |
| 94 | # calculate correction |
| 95 | error = current - setPoint |
| 96 | if Ki < 0.0001 and Ki > -0.0001: |
| 97 | integralArea = 0.0 |
| 98 | else: |
| 99 | integralArea += error |
| 100 | correction = Kp * error + Ki * integralArea + Kd * (error - previousError) |
| 101 | # print(Kp * error, Ki * integralArea, Kd * (error - previousError)) |
| 102 | previousError = error |
| 103 | |
| 104 | # calculate motor speedss |
| 105 | leftMotorSpeed = int(motorSpeed + correction) |
| 106 | rightMotorSpeed = int(motorSpeed - correction) |
| 107 | |
| 108 | if leftMotorSpeed == 0: leftMotorSpeed = 1 |
| 109 | if rightMotorSpeed == 0: rightMotorSpeed = 1 |
| 110 | # if leftMotorSpeed >= 300: leftMotorSpeed = 299 |
| 111 | # if rightMotorSpeed >= 300: rightMotorSpeed = 299 |
| 112 | |
| 113 | # update motor speeds |
| 114 | if stopMotors is False: |
| 115 | gpg.set_motor_dps(gpg.MOTOR_LEFT, dps=leftMotorSpeed) |
| 116 | gpg.set_motor_dps(gpg.MOTOR_RIGHT, dps=rightMotorSpeed) |
| 117 | |
| 118 | # make the loop work at a given frequency |
| 119 | end = time() |
| 120 | delayDiff = end - start |
| 121 | if loopPeriod - delayDiff > 0: |
| 122 | sleep(loopPeriod - delayDiff) |
| 123 | except Exception as err: |
| 124 | print(str(err)) |
| 125 | stopper.set() |
| 126 | finally: |
| 127 | gpg.stop() |
| 128 | |
| 129 | def Main(): |
| 130 |
nothing calls this directly
no test coverage detected