/*
 * SG90 serbo moter test routine
 *
 * GPIO18=SERVO yellow cable
 * TACK SWITCH=GPIO20,GPIO21
 */
#include <iostream>
#include <wiringPi.h>
#include <softPwm.h>
using namespace std;

int SERVO = 18;
/* 0-180 angle */
int set_angle(int angle) {
	if (angle < 0) return 0;
	if (angle >180) return 0;
	double duty;
	duty = angle / 9.8 + 5;
	softPwmWrite(SERVO, int(duty));
	return 1;
}

int main() {
	int TACK1 = 20;
	int TACK2 = 21;
	int checkData = 0;
	int inputData = 0;
	int deg = 90;
	int ret;

	if(wiringPiSetupGpio()<0) {
		cout << "wiring error\n";
		return 1;
	}
	pinMode(SERVO, OUTPUT);
	softPwmCreate(SERVO, deg, 100);
	pinMode(TACK1, INPUT);
	pinMode(TACK2, INPUT);
	pullUpDnControl(TACK1,PUD_DOWN);
	pullUpDnControl(TACK2,PUD_DOWN);

	set_angle(deg);
	while(true) {
		inputData = digitalRead(TACK1);
		delay(120);
		if(inputData==1 ) {
			deg+=10;
			ret=set_angle(deg);
			if (ret==0) deg -=10;
		}
		inputData = digitalRead(TACK2);
		delay(120);
		if(inputData==1 ) {
			deg-=10;
			ret=set_angle(deg);
			if (ret==0) deg +=10;
		}
	}

	return 0;
}