#include <SoftwareSerial.h>
#include "Kangaroo.h"
//#include "PositionControl.h"


#define TX_PIN 11
#define RX_PIN 10
// Independent mode channels on Kangaroo are, by default, '1' and '2'.
SoftwareSerial  SerialPort(RX_PIN, TX_PIN);
KangarooSerial  K(SerialPort);
KangarooChannel K1(K, '1');
KangarooChannel K2(K, '2');
int setP; // for incoming serial data

void setup()
{
	SerialPort.begin(9600);
	SerialPort.listen();
	Serial.begin(9600);
	K1.start();
	K1.home().wait();
	Serial.println(K1.getP().value());
}

// .wait() waits until the command is 'finished'. For position, this means it is within the deadband
// distance from the requested position. You can also call K1.p(position); without .wait() if you want to command it
// but not wait until it gets to the destination. If you do this, you may want to use K1.getP().value()
// to check progress.
void loop()
{
	//Serial.println("Starting");
	//K1.pi(1024).wait();
	//Serial.println(K1.getP().value());
	//delay(1000);
	//Serial.println(setP);
	while (Serial.available()==0){}
	setP = Serial.parseInt();
	Serial.println((setP));
	K1.pi(setP).wait();

	// Go to the minimum side at whatever speed limit is set on the potentiometers.
	/*K1.home().wait();

	 */
}