Revision 2:0f76226be922, committed 2015-07-23

Comitter:

kityann

Date:

Thu Jul 23 08:16:33 2015 +0000

Parent:

1:6a034792e059

Commit message:

2015/07/23

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/VNH5019.lib	Thu Jul 23 08:16:33 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/teams/aigamozu/code/VNH5019/#1bcdb655df71

--- 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

--- a/cansat.h	Thu Jul 16 07:56:54 2015 +0000
+++ b/cansat.h	Thu Jul 23 08:16:33 2015 +0000
@@ -5,13 +5,19 @@
  * Includes
  */
 #include "mbed.h"
-
+#include "VNH5019.h"
+enum STATUS {GPS_AVAIL = 0, GPS_UNAVAIL = 1,GPS_OUT_AREA = 2};
 
-
-const int distance_threshold[2]={0.1,0.5}
+const int distance_threshold[2]={0.1,0.5};
 
 class CanSat{
     public:
+        STATUS nowStatus;
+    
+        char motor_command;
+        void control_Motor(int,int);//動き、スピード
+    
+        CanSat(VNH5019 agzSheild);
         //ロボットとターゲットの方角からロボットの動きを決める
         char get_moving_angle(int robot_angle,int target_angle);
         //ロボットとターゲットの距離の計算
@@ -24,20 +30,43 @@
         bool is_stack();
         //ログデータの送信
         void log_send();
+        
+        //get関数
+        double get_robot_x();
+        double get_robot_y();
+        double get_robotKalman_x();
+        double get_robotKalman_y();
+        
+        //set関数
+        void set_robot_x(double);
+        void set_robot_y(double);   
+        void set_robotKalman_x(double);
+        void set_robotKalman_y(double);
     
+        void set_gyro(double x, double y, double z);
+        void set_compass(double x, double y, double z);
+        void set_pressure(double p);
+        void set_temperature(double t);
+        void set_humidity(double h);
+        void set_acceleration(double x, double y, double z);
+
     private:
         //モータ
-        //VNH5019 _agzSheild;
+        VNH5019 _agzSheild;
         //ロボットのスピード:32,64,128の3つ？
         int speed;
         //ロボットとターゲットの座標（緯度と経度）を表す
-        double robot_x,robot_y,target_x,target_y;
+        double robot_x,robot_y;
+        double robotK_x,robotK_y;//kalman filter
+        double target_x,target_y;
         //ロボットとターゲットの方角を表す。北を１として北東：２、東：３、南東４のように１～８の値を持つ
         int robot_angle,target_angle;
         //ジャイロ
-        double gyro_x,gyro_y,gyro_z;
+        int gyro_x,gyro_y,gyro_z;
         //コンパス
-        double compass_x,compass_y,compass_z;
+        int compass_x,compass_y,compass_z;
+        //加速度
+        double acceleration_x,acceleration_y,acceleration_z;
         //気圧計の気圧、気温、湿度
         double pressure,temperature,humidity;
         //ロボットの速度