Diff: cansat.cpp

Revision:

2:0f76226be922

Parent:

0:3f50511c1c1f

Child:

4:8713fff9e20d

--- a/cansat.cpp	Thu Jul 16 07:56:54 2015 +0000
+++ b/cansat.cpp	Thu Jul 23 08:16:33 2015 +0000
@@ -0,0 +1,92 @@
+#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
+            motor_command = 'f';
+        }
+        if(flag == 1){
+            _agzSheild.changeSpeed(0, speed, 0, speed); 
+            motor_command = 's';
+        }
+        if(flag == 2){
+             _agzSheild.changeSpeed(1, speed, 2, speed); //Turn Right
+            motor_command = 'r';
+        }           
+        if(flag == 3){
+             _agzSheild.changeSpeed(2, speed, 1, speed); //Turn Right
+            motor_command = 'l';
+        }   
+        if(flag == 4){
+            _agzSheild.changeSpeed(2, speed, 2, speed); 
+            motor_command = 'b';
+        }
+}
+    
+    
+//////////////////////////////
+//          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;
+}
+//////////////////////////////
+//          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(double x, double y, double z){
+    CanSat::compass_x = x;
+    CanSat::compass_y = y;
+    CanSat::compass_z = z;
+}
+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;
+}
\ No newline at end of file