Project-K9/DriveCog.cpp at master · MLenterman/Project-K9 · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
#include "DriveCog.h"

void driveCog(){
	PositionControl* positionControl = k9->positionControl;
	while(1){
		// Move forward
		if(positionControl->getMoveForwardFlag){
			int targetSpeed = positionControl->getTargetSpeed();
			drive_rampStep(targetSpeed, targetSpeed);

			// Update travelled ticks
			int* ticks;
			drive_getTicks(ticks, ticks);
			positionControl->setCurrentTicks(*ticks);
		}

		// Move backward
		if(positionControl->getMoveBackwardFlag){
			int targetSpeed = positionControl->getTargetSpeed();
			drive_rampStep(targetSpeed * (-1), targetSpeed * (-1));

			// Update travelled ticks
			int* ticks;
			drive_getTicks(ticks, ticks);
			positionControl->setCurrentTicks(*ticks);
		}

		// Stop
		if(positionControl->getStopFlag){
			drive_rampStep(0, 0);

			// Update travelled ticks
			int* ticks;
			drive_getTicks(ticks, ticks);
			positionControl->setCurrentTicks(*ticks);
		}
	}
}