// controller for the inverted pendulum
//

macro timeOut = 0.1;
macro K0 = 0.1020408;
macro K1 = 0.4081633;
macro K2 = 26.63102;
macro K3 = 4.2040816;

module Controller(hybrid real position, hybrid real velocity, hybrid real angle, hybrid real angularVelocity, hybrid real force) {

	hybrid real t;	
	t = 0.0;

	while(true) {
		// sample the plant
		// discrete controller
		next(t) = 0;	
		next(force) = position * K0 + velocity * K1 + angle * K2 + angularVelocity * K3;

		// to enable the plant to see the change
		flow {} until (true);
	
		// hold the actuator
		flow {
			drv(t) <- 1.0;

			// to maintain values
			drv(position) <- 0;
			drv(angle) <- 0;
			drv(velocity) <- 0;
			drv(angularVelocity) <- 0;	
			drv(force) <- 0;
		} until (cont(t) >= timeOut);
	}
}