sahaquiel_raspi_uart_test - skipper_raspi_uart_test

Users » imanomadao » Code » sahaquiel_raspi_uart_test
imano yuta / Mbed 2 deprecated sahaquiel_raspi_uart_test
skipper_raspi_uart_test
Dependencies: mbed MPU6050_2 HMC5883L_4 SDFileSystem3
Diff: main.cpp

Revision:: 17:83db05cab3d1
Parent:: 16:26cee2aaf61d
Child:: 18:4566d1f5fc60
--- a/main.cpp	Wed Feb 13 08:18:10 2019 +0000
+++ b/main.cpp	Thu Feb 14 09:09:25 2019 +0000
@@ -8,25 +8,20 @@
 //MPU_check用
 #define PI 3.14159265358979
 
-#define servo_NEUTRAL_R    1614
-#define servo_NEUTRAL_L    1621
+#define servo_NEUTRAL_R         1614
+#define servo_NEUTRAL_L         1621
 #define servo_slow_FORWARD_R    1560
 #define servo_slow_FORWARD_L    1560
-#define servo_low_FORWARD_R     1560   //RasPi速さ調節用 
 #define servo_high_FORWARD_R    1860
-#define servo_low_FORWARD_L     1560
 #define servo_high_FORWARD_L    1860
-#define servo_low_back_R        1360
-#define servo_high_back_R       1160
-#define servo_low_back_L        1360
-#define servo_high_back_L       1160
-#define turntable_NEUTRAL     1180 //カメラ台座のサーボ
-#define matchspeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
-#define focus_NEUTRAL  1455  //焦点合わせ用サーボ
-#define camera_deg_A 1400   //カメラ角度調整
-#define camera_deg_B 1800
-#define pint_speed 25
-#define pint_wait 3.5
+#define turntable_NEUTRAL       1180        //カメラ台座のサーボ
+#define matchspeed              1500 + 100  //カメラと方向を合わせるときの車輪の速度
+#define focus_NEUTRAL           1455        //焦点合わせ用サーボ
+#define camera_deg_A            1400        //カメラ角度調整
+#define camera_deg_B            1800
+#define pint_speed              25
+#define pint_wait               3.5
+#define turntable_speed         1800 
 
 #define MOVE_NEUTRAL    0
 #define MOVE_FORWARD    1   
@@ -63,7 +58,8 @@
             int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
             int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
             int *Camera_deg_A, int *Camera_deg_B,
-            int *Pint_speed, float *Pint_wait
+            int *Pint_speed, float *Pint_wait,
+            int *Turntable_speed
             );
 
 static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
@@ -101,6 +97,12 @@
 
 /*超音波はRaspberryPiに積む*/
 
+DigitalOut led1(PA_0);  //黄色のコネクタ
+DigitalOut led2(PA_1);
+DigitalOut led3(PB_5); 
+DigitalOut led4(PB_4);
+
+
 //外付けコンパス
 HMC5883L compass(PB_9, PB_8);    //コンパスセンサー TIM4_CH3とCH4
 
@@ -115,7 +117,7 @@
     //MPU_check
     setup();
     
-    servoCameradeg.pulsewidth_us(camera_deg_A);
+    servoCameradeg.pulsewidth_us(Camera_deg_A);
     
     // シリアル通信受信の割り込みイベント登録
     pc.attach(getSF_Serial_jevois, Serial::RxIrq);
@@ -147,51 +149,51 @@
     //NVIC_DisableIRQ(USART2_IRQn);
     switch(g_landingcommand){
         case 'N': //MOVE_NEUTRAL 
-            servoR.pulsewidth_us(servo_NEUTRAL_R);
-            servoL.pulsewidth_us(servo_NEUTRAL_L);
+            servoR.pulsewidth_us(Servo_NEUTRAL_R);
+            servoL.pulsewidth_us(Servo_NEUTRAL_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn);
             break;
             
         case 'Y': //MOVE_FORWARD
-            servoR.pulsewidth_us(servo_high_FORWARD_R);
-            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            servoR.pulsewidth_us(Servo_high_FORWARD_R);
+            servoL.pulsewidth_us(Servo_high_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
             
         case 'l': //MOVE_LEFT Low Speed
-            servoR.pulsewidth_us(servo_low_FORWARD_R);
+            servoR.pulsewidth_us(Servo_slow_FORWARD_R);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
         
         case 'L': //MOVE_LEFT High Speed
-            servoR.pulsewidth_us(servo_high_FORWARD_R);
+            servoR.pulsewidth_us(Servo_high_FORWARD_R);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             
         case 'r': //MOVE_RIGHT Low Speed  
-            servoL.pulsewidth_us(servo_low_FORWARD_L);
+            servoL.pulsewidth_us(Servo_slow_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
         
         case 'R': //MOVE_RIGHT  High Speed
-            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            servoL.pulsewidth_us(Servo_high_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
             
         case 'G': //GOAL_FORWARD
-            servoR.pulsewidth_us(servo_slow_FORWARD_R);
-            servoL.pulsewidth_us(servo_slow_FORWARD_L);
+            servoR.pulsewidth_us(Servo_slow_FORWARD_R);
+            servoL.pulsewidth_us(Servo_slow_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
         
         case '1': //MOVE_FORWARD Speed Level 1
-            servoR.pulsewidth_us(servo_high_FORWARD_R);
-            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            servoR.pulsewidth_us(Servo_high_FORWARD_R);
+            servoL.pulsewidth_us(Servo_high_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             wait(1);
             do{
@@ -200,8 +202,8 @@
             break;
         
         case '2': //MOVE_FORWARD Speed Level 2
-            servoR.pulsewidth_us(servo_high_FORWARD_R);
-            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            servoR.pulsewidth_us(Servo_high_FORWARD_R);
+            servoL.pulsewidth_us(Servo_high_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             wait(2);
             do{
@@ -210,8 +212,8 @@
             break;
         
         case '3': //MOVE_FORWARD Speed Level 3
-            servoR.pulsewidth_us(servo_high_FORWARD_R);
-            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            servoR.pulsewidth_us(Servo_high_FORWARD_R);
+            servoL.pulsewidth_us(Servo_high_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             wait(3);
             do{
@@ -220,8 +222,8 @@
             break;
         
         case '4': //MOVE_FORWARD Speed Level 4
-            servoR.pulsewidth_us(servo_high_FORWARD_R);
-            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            servoR.pulsewidth_us(Servo_high_FORWARD_R);
+            servoL.pulsewidth_us(Servo_high_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             wait(4);
             do{
@@ -230,8 +232,8 @@
             break;
             
         case '5': //MOVE_FORWARD Speed Level 5
-            servoR.pulsewidth_us(servo_high_FORWARD_R);
-            servoL.pulsewidth_us(servo_high_FORWARD_L);
+            servoR.pulsewidth_us(Servo_high_FORWARD_R);
+            servoL.pulsewidth_us(Servo_high_FORWARD_L);
             //NVIC_EnableIRQ(USART1_IRQn);
             wait(5);
             do{
@@ -240,7 +242,7 @@
             break;
             
         case 'M': //MatchPosition
-            servoR.pulsewidth_us(matchspeed);
+            servoR.pulsewidth_us(Matchspeed);
             //NVIC_EnableIRQ(USART1_IRQn);
             //NVIC_EnableIRQ(USART2_IRQn); 
             break;
@@ -267,22 +269,22 @@
 void MoveCameraBoard(){
     MoveCansat('N');
     g_landingcommand='N';
-    servoTurnTable.pulsewidth_us(1800);
+    servoTurnTable.pulsewidth_us(Turntable_speed);
     wait_ms(600);
-    servoTurnTable.pulsewidth_us(turntable_NEUTRAL);
+    servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
     if(jevoisFlag == true) FocusAdjust();
     else wait(1);
     
     if(g_landingcommand!='N') return;
     if(!CameraDegFlag){
-        for(int i=0; i<camera_deg_B-camera_deg_A;i=i+10){
-            servoCameradeg.pulsewidth_us(camera_deg_A+10);
+        for(int i=0; i<Camera_deg_B-Camera_deg_A;i=i+10){
+            servoCameradeg.pulsewidth_us(Camera_deg_A+10);
             wait_ms(20);
             }
         CameraDegFlag=!CameraDegFlag;
         }else{
-            for(int i=0; i<camera_deg_B-camera_deg_A;i=i+10){
-            servoCameradeg.pulsewidth_us(camera_deg_A-10);
+            for(int i=0; i<Camera_deg_B-Camera_deg_A;i=i+10){
+            servoCameradeg.pulsewidth_us(Camera_deg_A-10);
             }
         CameraDegFlag = !CameraDegFlag;
     }
@@ -306,14 +308,14 @@
 }
 
 void FocusAdjust(){
-    servoCameraPinto.pulsewidth_us(focus_NEUTRAL-200);
+    servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
     //pc.printf("set\r\n");
     wait(1);
     
-    servoCameraPinto.pulsewidth_us(focus_NEUTRAL+pint_speed);
+    servoCameraPinto.pulsewidth_us(Focus_NEUTRAL+Pint_speed);
     //pc.printf("check\r\n");
     wait(pint_wait);
-    servoCameraPinto.pulsewidth_us(focus_NEUTRAL);
+    servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
       
   return;      
 }     
@@ -423,8 +425,8 @@
                &Servo_slow_FORWARD_R,&Servo_slow_FORWARD_L,
                &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL,
                &Camera_deg_A, &Camera_deg_B,
-               &Pint_speed,&Pint_wait
-               
+               &Pint_speed,&Pint_wait,
+               &Turntable_speed
                );
      
     Init_sensors();
@@ -587,7 +589,8 @@
                int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
                int *Turntable_NEUTRAL,int *Matchspeed,int *Focus_NEUTRAL,
                int *Camera_deg_A, int *Camera_deg_B,
-               int *Pint_speed, float *Pint_wait
+               int *Pint_speed, float *Pint_wait,
+               int *Turntable_speed
                ){
 
     pc.printf("SDsetup start.\r\n");    
@@ -608,7 +611,8 @@
         "CAMERA_DEG_A",
         "CAMERA_DEG_B",
         "PINT_SPEED",
-        "PINT_WAIT"
+        "PINT_WAIT",
+        "TURNTABLE_SPEED"
     };
 
     fp = fopen("/sd/option.txt","r");
@@ -668,7 +672,10 @@
         else{                                         *Pint_wait = pint_wait;
                                                       SDerrorcount++;
         }
-        
+         if(GetParameter(fp,paramNames[13],parameter)) *Turntable_speed = atof(parameter);
+        else{                                         *Turntable_speed = turntable_speed;
+                                                      SDerrorcount++;
+        }
         fclose(fp);
 
     }else{  //ファイルがなかったら
@@ -687,6 +694,7 @@
         *Camera_deg_B = camera_deg_B;
         *Pint_speed = pint_speed;
         *Pint_wait = pint_wait;
+        *Turntable_speed = turntable_speed;
         SDerrorcount = -1;
     }
     pc.printf("SDsetup finished.\r\n");
@@ -701,6 +709,7 @@
     pc.printf("Focus_NEUTRAL = %d\r\n", *Focus_NEUTRAL);
     pc.printf("Camera_deg_A = %d, Camera_deg_B =  %d\r\n", *Camera_deg_A, *Camera_deg_B);
     pc.printf("Pint_speed = %d, Pint_wait =  %f\r\n", *Pint_speed, *Pint_wait);
+    pc.printf("Turntable_speed = %d\r\n",*Turntable_speed);
     
     return SDerrorcount;
 }
Repository toolbox

Export to desktop IDE
Build repository
Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	06 Feb 2020
Imports:	1
Forks:	0
Commits:	43
Dependents:	0
Dependencies:	4
Followers:	1
Diff: main.cpp

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
