HABET_SolarEclipse/movegimbal.cpp at master · tuftedal/HABET_SolarEclipse · GitHub

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
#include <commands.h>
//#include <Arduino.h>
#include <WProgram.h>
#include <string.h>
// The main serial interface
extern HardwareSerial *hardSerial;
extern Servo pitch;
extern Servo roll;
extern Servo yaw;

void GimbalMove(char* arg_){

	//parse the chars here
	//need to seperate into 3 integers
	//verify that they are all integers
	char* val1;
	char* val2;
	char* val3;
	int* pitchdeg;
	int* rolldeg;
	int* yawdeg;

	val1=strtok(arg_,",");
	val2=strtok(NULL,",");
	val3=strtok(NULL,",");
	pitchdeg = atoi(val1);
	rolldeg = atoi(val2);
	yawdeg = atoi(val3);

	if(val1==NULL){
		return;
	}
	if(val2==NULL){
		return;
	}
	if(val3==NULL){
		return;
	}

	pitch.write(ptichdeg);
	roll.write(rolldeg);
	yaw.write(yawdeg);


}