航空研究会
初期値を調整
Dependencies: mbed MPU6050_2 HMC5883L_2
Diff: main.cpp
- Revision:
- 12:a2c6207cbce3
- Parent:
- 11:d44d137831b9
--- a/main.cpp Sun Feb 10 18:13:37 2019 +0000
+++ b/main.cpp Tue Feb 12 02:59:22 2019 +0000
@@ -7,17 +7,19 @@
#define servo_NEUTRAL_R 1614
#define servo_NEUTRAL_L 1621
-#define servo_FORWARD_R 1860
-#define servo_FORWARD_L 1860
-#define servo_back_R 1060
-#define servo_back_L 1060
#define servo_slow_FORWARD_R 1560
#define servo_slow_FORWARD_L 1560
-#define servo_slow_back_R 1360
-#define servo_slow_back_L 1360
+#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 Focus_NEUTRAL 1455 //焦点合わせ用サーボ
#define MOVE_NEUTRAL 0
#define MOVE_FORWARD 1
@@ -30,6 +32,17 @@
#define MAX_FORWARD 8 //はやい_姿勢修正用
#define MAX_BACK 9
+int Servo_NEUTRAL_R; //SDcard用宣言
+int Servo_NEUTRAL_L;
+int Servo_FORWARD_R;
+int Servo_FORWARD_L;
+int Servo_back_R;
+int Servo_back_L;
+int Servo_slow_FORWARD_R;
+int Servo_slow_FORWARD_L;
+int Turntable_NEUTRAL;
+int Matchspeed;
+int Minfocus;
void getSF_Serial_jevois();
void getSF_Serial_pi();
@@ -47,6 +60,16 @@
void MatchPosition();
void FocusAdjust();
+//sd設定
+int GetParameter(FILE *fp, const char *paramName,char parameter[]);
+
+int SetOptions(int *Servo_NEUTRAL_R,int *Servo_NEUTRAL_L,
+ int *Servo_FORWARD_R,int *Servo_FORWARD_L,
+ int *Servo_back_R,int *Servo_back_L,
+ int *Servo_slow_FORWARD_R,int *Servo_slow_FORWARD_L,
+ int *Turntable_NEUTRAL,int *Matchspeed,int *Minfocus
+ );
+
static float nowAngle[3] = {0,0,0},nowAngle_HMC=0;
float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
@@ -60,7 +83,7 @@
RawSerial pc(PA_2,PA_3,115200); //uart2
//pa2:UART2_TX,pa3:UART2_RX
-//RawSerial pc2(PB_6,PB_7,115200); //uart1
+RawSerial pc2(PA_11,PB_12,115200); //uart6
//pb6:UART1_TX,pb7:UART1_RX
//PWM pin宣言
@@ -71,7 +94,7 @@
PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
PwmOut servoCameramount(PA_6); //skipperウラ カメラマウント起動
PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作
-PwmOut servoParachute(PA_11); //skipper USB端子より パラシュート切り離し
+//PwmOut servoParachute(PA_11); //skipper USB端子より パラシュート切り離し
/*通信用のpinは
PA_3(UART2_Rx) skipperウラ
@@ -99,7 +122,7 @@
// シリアル通信受信の割り込みイベント登録
pc.attach(getSF_Serial_jevois, Serial::RxIrq);
- //pc2.attach(getSF_Serial_pi, Serial::RxIrq);
+ pc2.attach(getSF_Serial_pi, Serial::RxIrq);
while(1) {
if(jevoisFlag==true) NVIC_DisableIRQ(USART1_IRQn);
@@ -134,28 +157,30 @@
break;
case 'Y': //MOVE_FORWARD
- servoR.pulsewidth_us(servo_FORWARD_R);
- servoL.pulsewidth_us(servo_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
- servoR.pulsewidth_us(servo_slow_FORWARD_R);
- servoL.pulsewidth_us(servo_slow_back_L);
+ case 'l': //MOVE_LEFT Low Speed
+ servoR.pulsewidth_us(servo_low_FORWARD_R);
+ //NVIC_EnableIRQ(USART1_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
+
+ case 'L': //MOVE_LEFT High Speed
+ servoR.pulsewidth_us(servo_high_FORWARD_R);
//NVIC_EnableIRQ(USART1_IRQn);
//NVIC_EnableIRQ(USART2_IRQn);
- case 'R': //MOVE_RIGHT
- servoR.pulsewidth_us(servo_slow_back_R);
- servoL.pulsewidth_us(servo_slow_FORWARD_L);
+ case 'r': //MOVE_RIGHT Low Speed
+ servoL.pulsewidth_us(servo_low_FORWARD_L);
//NVIC_EnableIRQ(USART1_IRQn);
//NVIC_EnableIRQ(USART2_IRQn);
break;
-
- case 'B': //MOVE_BACK
- servoR.pulsewidth_us(servo_back_R);
- servoL.pulsewidth_us(servo_back_L);
+
+ case 'R': //MOVE_RIGHT High Speed
+ servoL.pulsewidth_us(servo_high_FORWARD_L);
//NVIC_EnableIRQ(USART1_IRQn);
//NVIC_EnableIRQ(USART2_IRQn);
break;
@@ -166,10 +191,50 @@
//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);
+ //NVIC_EnableIRQ(USART1_IRQn);
+ wait(1);
+ do{
+ }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
+ //NVIC_EnableIRQ(USART2_IRQn);
+ break;
+
+ case '2': //MOVE_FORWARD Speed Level 2
+ servoR.pulsewidth_us(servo_high_FORWARD_R);
+ servoL.pulsewidth_us(servo_high_FORWARD_L);
+ //NVIC_EnableIRQ(USART1_IRQn);
+ wait(2);
+ do{
+ }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
+ //NVIC_EnableIRQ(USART2_IRQn);
+ break;
+
+ case '3': //MOVE_FORWARD Speed Level 3
+ servoR.pulsewidth_us(servo_high_FORWARD_R);
+ servoL.pulsewidth_us(servo_high_FORWARD_L);
+ //NVIC_EnableIRQ(USART1_IRQn);
+ wait(3);
+ do{
+ }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
+ //NVIC_EnableIRQ(USART2_IRQn);
+ break;
+
+ case '4': //MOVE_FORWARD Speed Level 4
+ servoR.pulsewidth_us(servo_high_FORWARD_R);
+ servoL.pulsewidth_us(servo_high_FORWARD_L);
+ //NVIC_EnableIRQ(USART1_IRQn);
+ wait(4);
+ do{
+ }while((nowAngle[PITCH]<20&&nowAngle[PITCH]>-20)&&(nowAngle[ROLL]<20&&nowAngle[ROLL]>-20));
+ //NVIC_EnableIRQ(USART2_IRQn);
+ break;
- case 'J': //MOVE_FORWARD Tim
- servoR.pulsewidth_us(servo_FORWARD_R);
- servoL.pulsewidth_us(servo_FORWARD_L);
+ case '5': //MOVE_FORWARD Speed Level 5
+ servoR.pulsewidth_us(servo_high_FORWARD_R);
+ servoL.pulsewidth_us(servo_high_FORWARD_L);
//NVIC_EnableIRQ(USART1_IRQn);
wait(5);
do{
@@ -183,10 +248,14 @@
//NVIC_EnableIRQ(USART2_IRQn);
break;
- case 'T': //jevoisからraspberry piへの切り替え
+ case 'P': //jevoisからraspberry piへの切り替え
jevoisFlag = false;
//NVIC_EnableIRQ(USART2_IRQn);
break;
+
+ case 'S':
+ /*RasPiからの超音波判定(プログラムスタート部)*/
+ break;
default :
//NVIC_EnableIRQ(USART1_IRQn);
@@ -295,7 +364,7 @@
}
-/*void getSF_Serial_pi(){
+void getSF_Serial_pi(){
//NVIC_DisableIRQ(USART2_IRQn);
@@ -342,7 +411,7 @@
//NVIC_EnableIRQ(USART2_IRQn);
-}*/
+}
void setup(){
@@ -501,6 +570,8 @@
flg_checkoutlier = false;
}
+
+
void DebugPrint(){
//for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用