File content as of revision 11:e82a5e1154a1:

#include "cansat.h"


//////////////////////////////
//          Init            //
//////////////////////////////
CanSat::CanSat(VNH5019 agzSheild):_agzSheild(agzSheild){

    }
    
    
    
void CanSat::control_Motor(int flag,int speed){

        if(flag == 0){
            _agzSheild.changeSpeed(1, speed, 1, speed); //straight
        }
        if(flag == 1){
            _agzSheild.changeSpeed(0, speed, 0, speed); 
        }
        if(flag == 2){
             _agzSheild.changeSpeed(1, speed, 0, speed/2); //Turn Right
        }           
        if(flag == 3){
             _agzSheild.changeSpeed(0, speed/2, 1, speed); //Turn Right
        }   
        if(flag == 4){
            _agzSheild.changeSpeed(2, speed, 2, speed); 
        }
}
    
    
//////////////////////////////
//          Get             //
//////////////////////////////   
double CanSat::get_robot_x(){
    return robot_x;
}
double CanSat::get_robot_y(){
    return robot_y;
}
double CanSat::get_robotKalman_x(){
    return robotK_x;
}
double CanSat::get_robotKalman_y(){
    return robotK_y;
}
int16_t CanSat::get_compass_x(){
    return compass_x;
}
int16_t CanSat::get_compass_y(){
    return compass_y;
}
int16_t CanSat::get_compass_z(){
    return compass_z;
}
double CanSat::get_compass_angle(){
    return compass_angle;
}
double CanSat::get_target_distance(){
    return target_distance;
}
double CanSat::get_target_x(){
    return target_x;
}
double CanSat::get_target_y(){
    return target_y;
}
int CanSat::get_speed(){
    return speed;
}
int CanSat::get_robot_angle(){
    return robot_angle;
}
int CanSat::get_target_angle(){
    return target_angle;
}
double CanSat::get_pressure(){
    return pressure;
}
double CanSat::get_temperature(){
    return temperature;
} 
double CanSat::get_humidity(){
    return humidity;
}
//////////////////////////////
//          Set             //
//////////////////////////////
void CanSat::set_robot_x(double x){
    robot_x = x;
}
void CanSat::set_robot_y(double y){
    robot_y = y;
}

void CanSat::set_robotKalman_x(double cur_x){
    robotK_x = cur_x;
}
void CanSat::set_robotKalman_y(double cur_y){
    robotK_y = cur_y;
}
void CanSat::set_gyro(double x, double y, double z){
    CanSat::gyro_x = x;
    CanSat::gyro_y = y;
    CanSat::gyro_z = z;    
}
void CanSat::set_compass(int16_t x, int16_t y, int16_t z, double angle){
   double setAngle;
   CanSat::compass_x = x;
   CanSat::compass_y = y;
   CanSat::compass_z = z;
   
   setAngle = angle-270;
   if(setAngle < 0){
       setAngle = setAngle + 360;
   }
   setAngle = 360 - setAngle; 
   
   CanSat::compass_angle = setAngle;    
}
void CanSat::set_pressure(double p){
    CanSat::pressure = p;
}
void CanSat::set_temperature(double t){
    CanSat::temperature = t;
}
void CanSat::set_humidity(double h){
    CanSat::humidity = h;
}
void CanSat::set_acceleration(double x, double y, double z){
    CanSat::acceleration_x = x;
    CanSat::acceleration_y = y;
    CanSat::acceleration_z = z;
}
void CanSat::set_target_distance(double distance){
    CanSat::target_distance = distance;
}
void CanSat::set_speed(int new_speed){
    CanSat::speed = new_speed;
}
void CanSat::set_robot_angle(int new_robot_angle){
    CanSat::robot_angle = new_robot_angle;
}
void CanSat::set_target_angle(int new_target_angle){
    CanSat::target_angle = new_target_angle;
}
void CanSat::set_target(double x, double y){
    CanSat::target_x = x;
    CanSat::target_y = y;
}