航空研究会
/
sahaquiel_brushless_tset
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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;
}