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
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
#include <Servo.h>

const int servos[] = {P2_6, P2_1, P2_2, P2_5, P1_6};
Servo servo0;
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;


void setup() {
  // put your setup code here, to run once:
	servo0.attach(servos[0]);
	servo1.attach(servos[1]);
	servo2.attach(servos[2]);
	servo3.attach(servos[3]);
	servo4.attach(servos[4]);
	Serial.begin(9600);
	
}

String inputString = "";
boolean stringComplete = false;
int num;
String subs;

int strtoint(String str){
	char char_string[str.length()+1];
	str.toCharArray(char_string, str.length()+1);
 
	return atoi(char_string);
}

void computerControl(){
	//for testing purposes, computer controll!
	boolean go = true;
	while (go){
		serialEvent();
		if (stringComplete){
			Serial.println(inputString);
			switch (inputString[0]) {
				case 't':
					//Servo
					subs = inputString.substring(2, inputString.length()-1);
					num = strtoint(subs);
					//Replace this mess with a hashtable, or something simpler
					switch (inputString[1]){
						case '0': 
							servo0.write(num);
							break;
						case '1':
							servo1.write(num);
							break;
						case '2':
							servo2.write(num);
							break;
						case '3':
							servo3.write(num);
							break;
						case '4':
							servo4.write(num);
							break;
					}
					break;
			}
			inputString = "";
			stringComplete = false;
		}
	}
}

void serialEvent(){
	while (Serial.available()){
		char inChar = (char)Serial.read();
		inputString += inChar;
		if (inChar == ';') {
			stringComplete = true;
		}
	}
}

void loop() {
  // put your main code here, to run repeatedly: 
  computerControl();
}