Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #define verPin 3
- #define horPin 5
- #include <string.h>
- #include <Servo.h>
- unsigned minHorDegree = 45;
- unsigned maxHorDegree = 135;
- unsigned minVerDegree = 45;
- unsigned maxVerDegree = 135;
- Servo srvHor;
- Servo srvVer;
- unsigned pos;
- char message[20] = "";
- unsigned index = 0;
- char aux[4] = "";
- int horValue = (maxHorDegree-minHorDegree)/2;
- int verValue = (maxVerDegree-minVerDegree)/2;
- int curHorValue = horValue;
- int curVerValue = verValue;
- int horMaxStep = 5;
- int verMaxStep = 5;
- void setup(){
- Serial.begin(9600);
- Serial.println("setup");
- srvHor.attach(horPin);
- srvVer.attach(verPin);
- Serial.println("servos attached");
- }
- void loop(){
- if(Serial.available() > 0){
- message[index] = Serial.read();
- if(message[index] != '\n'){
- Serial.print(message[index]);
- Serial.println(" is not null");
- index++;
- }else{
- Serial.println("Got enter. EOM");
- message[index] = '\0';
- index = 0;
- unsigned len = strlen(message);
- if(message[1] != ':'){
- Serial.print("error(");
- Serial.print(len);
- Serial.println(")");
- }else{
- Serial.print("ack(");
- Serial.print(len);
- Serial.println(")");
- if(message[0] == 'a'){ //a:...
- //ignore
- }else if(message[0] == 's'){ //s:XXX:XXX
- aux[0] = message[2];
- aux[1] = message[3];
- aux[2] = message[4];
- aux[3] = '\0';
- horValue = atoi(aux);
- if(horValue > maxHorDegree){
- horValue = maxHorDegree;
- }else if(horValue < minHorDegree){
- horValue = minHorDegree;
- }
- aux[6] = message[4];
- aux[7] = message[5];
- aux[8] = message[6];
- aux[3] = '\0';
- verValue = atoi(aux);
- if(verValue > maxVerDegree){
- verValue = maxVerDegree;
- }else if(horValue < minHorDegree){
- verValue = minVerDegree;
- }
- Serial.print(horValue);
- Serial.print(":");
- Serial.print(verValue);
- Serial.println(".");
- }else{
- Serial.println("unknown");
- }
- }
- }
- }
- if(curVerValue != verValue){
- int diff = verValue-curVerValue;
- Serial.print("verDiff should be: ");
- Serial.println(diff);
- if(diff > 0 && diff > verMaxStep){
- Serial.println("diff set to 5");
- diff = 5;
- }
- if(diff < 0 && diff < verMaxStep){
- Serial.println("diff set to -5");
- diff = -5;
- }
- curVerValue += diff;
- srvVer.write(curVerValue);
- Serial.print("ver: from ");
- Serial.print(curVerValue-diff);
- Serial.print(" to ");
- Serial.print(curVerValue);
- Serial.print(" until ");
- Serial.println(verValue);
- }
- if(0){//if(curHorValue != horValue){
- int diff = horValue - curHorValue;
- if(diff > horMaxStep){
- diff = horMaxStep;
- }
- if(diff < (-1)*horMaxStep){
- diff = (-1)*horMaxStep;
- }
- curHorValue += diff;
- srvHor.write(curHorValue);
- Serial.print("hor:set to ");
- Serial.println(curHorValue);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement