Created
June 27, 2018 16:39
-
-
Save Heasummn/c64fdc0a804dea16e375899ecc0ac503 to your computer and use it in GitHub Desktop.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| static const float kp = 10; | |
| void move(int leftPower, int rightPower) { | |
| motor[left1] = leftPower; | |
| motor[left2] = leftPower; | |
| motor[right1] = rightPower; | |
| motor[right2] = rightPower; | |
| } | |
| float driveForward(int power, float distance) { | |
| SensorValue[leftEncoder] = 0; | |
| SensorValue[rightEncoder] = 0; | |
| int error = 0; | |
| int slavePower = power; // subtract based upon observations | |
| float ticksNeeded = (360 / CIRC) * distance; | |
| float totalTicks = 0.0; | |
| while(abs(totalTicks) < ticksNeeded) { | |
| move(slavePower, power); | |
| error = SensorValue[rightEncoder] - SensorValue[leftEncoder]; | |
| slavePower += (int) (error / kp); | |
| writeDebugStreamLine("Moved: %f", (totalTicks * CIRC)/360); | |
| SensorValue[leftEncoder] = 0; | |
| SensorValue[rightEncoder] = 0; | |
| wait1Msec(20); | |
| totalTicks += (SensorValue[rightEncoder] + SensorValue[leftEncoder])/2; | |
| } | |
| writeDebugStreamLine("Moved: %f", (totalTicks * CIRC)/360); | |
| move(0, 0); | |
| return totalTicks; | |
| } | |
| task autonomous() | |
| { | |
| float distance = driveForward(70, 15); | |
| wait1Msec(500); | |
| writeDebugStreamLine("Total Moved: %f", (distance * CIRC)/360); | |
| } |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Moved: 0.000000 | |
| Moved: 0.000000 | |
| Moved: 0.000000 | |
| Moved: 0.108778 | |
| Moved: 0.362592 | |
| Moved: 0.688925 | |
| Moved: 1.087776 | |
| Moved: 1.450368 | |
| Moved: 1.849220 | |
| Moved: 2.284330 | |
| Moved: 2.755700 | |
| Moved: 3.190810 | |
| Moved: 3.734699 | |
| Moved: 4.278586 | |
| Moved: 4.822475 | |
| Moved: 5.402622 | |
| Moved: 5.982770 | |
| Moved: 6.562917 | |
| Moved: 7.179323 | |
| Moved: 7.831989 | |
| Moved: 8.484655 | |
| Moved: 9.137321 | |
| Moved: 9.753727 | |
| Moved: 10.333875 | |
| Moved: 10.914021 | |
| Moved: 11.421651 | |
| Moved: 12.038057 | |
| Moved: 12.654464 | |
| Moved: 13.234611 | |
| Moved: 13.887278 | |
| Moved: 14.467424 | |
| Moved: 15.083831 | |
| Total Moved: 15.083831 |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment