testing2

//Begin page main
//Declare any variables shared between functions here

float pos[12];
float forces[3];
float kp;
float myState[12];
int step;
float myPos[3];
float posA[3];
float posB[3];
float posC[3];

void init(){
	//This function is called once when your code is first loaded.

	//IMPORTANT: make sure to set any variables that need an initial value.
	//Do not assume variables will be set to 0 automatically!
	posA[0]=0.35;
	posA[1]=0.35;
	posA[2]=0.0;
	
	posB[0]=-0.35;
	posB[1]=0.35;
	posB[2]=0.0;
	
	posC[0]=0.15;
	posC[1]=0.15;
	posC[2]=0.0;
	
	step = 0;
		
	kp = 0.35;// coeficiente de velocidad. error contra velocidad, mayor más rápido
}

void goTo(float* dest){
    api.getMyZRState(pos);
	int i = 0;
	for(i = 0; i < 3; i++){
	    float error = dest[i] - pos[i];
	    if(error < 0.01){
	        error = 0;
	    }
	    forces[i] = kp * error;
	}
	api.setVelocityTarget(forces);
}

void loop(){
	//This function is called once per second.  Use it to control the satellite.
	goTo(posA);
	goTo(posB);
	goTo(posC);
	
}