航空研究会
SDコンパイル済み MoveCansat新コマンド追加 LED実装
Dependencies: mbed MPU6050_2 HMC5883L_2 SDFileSystem3
Revision 12:08ba6996246b, committed 2019-02-11
- Comitter:
- HARUKIDELTA
- Date:
- Mon Feb 11 12:50:29 2019 +0000
- Parent:
- 11:8427ecccf07d
- Commit message:
- SD
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Feb 11 10:58:57 2019 +0000
+++ b/main.cpp Mon Feb 11 12:50:29 2019 +0000
@@ -16,6 +16,14 @@
#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 1500 //カメラ台座のサーボ
#define MatchSpeed 1500 + 100 //カメラと方向を合わせるときの車輪の速度
#define minFocus 1200 //焦点合わせ用サーボの最小値
@@ -31,6 +39,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();
@@ -51,12 +70,12 @@
//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
- );
+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;
@@ -102,6 +121,11 @@
//MPU_check用
MPU6050DMP6 mpu6050(PC_0,&pc);
+//LED
+DigitalOut led1(PA_0); //黄色のコネクタ
+DigitalOut led2(PA_1);
+DigitalOut led3(PB_5);
+DigitalOut led4(PB_4);
int main() {
@@ -131,7 +155,6 @@
}
}
-
void MoveCansat(char g_landingcommand)
{
//NVIC_DisableIRQ(USART1_IRQn);
@@ -151,15 +174,28 @@
//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);
+ servoL.pulsewidth_us(servo_low_back_L);
+ //NVIC_EnableIRQ(USART1_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
+
+ case 'L': //MOVE_LEFT High Speed
+ servoR.pulsewidth_us(servo_high_FORWARD_R);
+ servoL.pulsewidth_us(servo_high_back_L);
//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
+ servoR.pulsewidth_us(servo_low_back_R);
+ servoL.pulsewidth_us(servo_low_FORWARD_L);
+ //NVIC_EnableIRQ(USART1_IRQn);
+ //NVIC_EnableIRQ(USART2_IRQn);
+ break;
+
+ case 'R': //MOVE_RIGHT High Speed
+ servoR.pulsewidth_us(servo_high_back_R);
+ servoL.pulsewidth_us(servo_high_FORWARD_L);
//NVIC_EnableIRQ(USART1_IRQn);
//NVIC_EnableIRQ(USART2_IRQn);
break;
@@ -177,8 +213,48 @@
//NVIC_EnableIRQ(USART1_IRQn);
//NVIC_EnableIRQ(USART2_IRQn);
break;
+
+ case '1': //MOVE_FORWARD Speed Level 1
+ servoR.pulsewidth_us(servo_FORWARD_R);
+ servoL.pulsewidth_us(servo_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_FORWARD_R);
+ servoL.pulsewidth_us(servo_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_FORWARD_R);
+ servoL.pulsewidth_us(servo_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_FORWARD_R);
+ servoL.pulsewidth_us(servo_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
+ case '5': //MOVE_FORWARD Speed Level 5
servoR.pulsewidth_us(servo_FORWARD_R);
servoL.pulsewidth_us(servo_FORWARD_L);
//NVIC_EnableIRQ(USART1_IRQn);
@@ -194,10 +270,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);
@@ -209,6 +289,7 @@
return;
}
+
void MoveCameraBoard(){
MoveCansat('N');
g_landingcommand='N';
@@ -355,11 +436,11 @@
void setup(){
-
+/*
SetOptions(&servo_NEUTRAL_R, &servo_NEUTRAL_L,&servo_FORWARD_R,&servo_FORWARD_L,&servo_back_R,&servo_back_L,&servo_slow_FORWARD_R,
&servo_slow_FORWARD_L,&TurnTable_NEUTRAL,&MatchSpeed,&minFocus
);
-
+*/
Init_sensors();
//switch2.rise(ResetTrim);
@@ -512,15 +593,16 @@
count_changeRPY = 0;
}else count_changeRPY++;
flg_checkoutlier = false;
-
+
}
-/sdによる設定
-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
+//sdによる設定
+
+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
){
pc.printf("SDsetup start.\r\n");
@@ -546,49 +628,49 @@
if(fp != NULL){ //開けたら
pc.printf("File was openned.\r\n");
- if(GetParameter(fp,paramNames[0],parameter)) *servo_NEUTRAL_R = atof(parameter);
- else{ *servo_NEUTRAL_R = SERVO_NEUTRAL_R;
+ if(GetParameter(fp,paramNames[0],parameter)) *Servo_NEUTRAL_R = atof(parameter);
+ else{ *Servo_NEUTRAL_R = servo_NEUTRAL_R;
SDerrorcount++;
}
- if(GetParameter(fp,paramNames[1],parameter)) *servo_NEUTRAL_L = atof(parameter);
- else{ *servo_NEUTRAL_L = SERVO_NEUTRAL_L;
+ if(GetParameter(fp,paramNames[1],parameter)) *Servo_NEUTRAL_L = atof(parameter);
+ else{ *Servo_NEUTRAL_L = servo_NEUTRAL_L;
SDerrorcount++;
}
- if(GetParameter(fp,paramNames[2],parameter)) *servo_FORWARD_R = atof(parameter);
- else{ *servo_FORWARD_R = SERVO_FORWARD_R;
+ if(GetParameter(fp,paramNames[2],parameter)) *Servo_FORWARD_R = atof(parameter);
+ else{ *Servo_FORWARD_R = servo_FORWARD_R;
SDerrorcount++;
}
- if(GetParameter(fp,paramNames[3],parameter)) *servo_FORWARD_L = atof(parameter);
- else{ *servo_FORWARD_L = SERVO_FORWARD_L;
+ if(GetParameter(fp,paramNames[3],parameter)) *Servo_FORWARD_L = atof(parameter);
+ else{ *Servo_FORWARD_L = servo_FORWARD_L;
SDerrorcount++;
}
- if(GetParameter(fp,paramNames[4],parameter)) *servo_back_R = atof(parameter);
- else{ *servo_back_R = SERVO_BACK_R;
+ if(GetParameter(fp,paramNames[4],parameter)) *Servo_back_R = atof(parameter);
+ else{ *Servo_back_R = servo_back_R;
SDerrorcount++;
}
- if(GetParameter(fp,paramNames[5],parameter)) *servo_back_L = atof(parameter);
- else{ *servo_back_L = SERVO_BACK_L;
+ if(GetParameter(fp,paramNames[5],parameter)) *Servo_back_L = atof(parameter);
+ else{ *Servo_back_L = servo_back_L;
SDerrorcount++;
}
- if(GetParameter(fp,paramNames[6],parameter)) *servo_slow_FORWARD_R = atof(parameter);
- else{ *servo_slow_FORWARD_R = SERVO_SLOW_FORWARD_R;
+ if(GetParameter(fp,paramNames[6],parameter)) *Servo_slow_FORWARD_R = atof(parameter);
+ else{ *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
SDerrorcount++;
}
- if(GetParameter(fp,paramNames[7],parameter)) *servo_slow_FORWARD_L = atof(parameter);
- else{ *servo_slow_FORWARD_L = SERVO_SLOW_FORWARD_L
+ if(GetParameter(fp,paramNames[7],parameter)) *Servo_slow_FORWARD_L = atof(parameter);
+ else{ *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
SDerrorcount++;
}
- if(GetParameter(fp,paramNames[10],parameter)) *TurnTable_NEUTRAL = atof(parameter);
- else{ *TurnTable_NEUTRAL = TURNTABLE_NEUTRAL;
+ if(GetParameter(fp,paramNames[8],parameter)) *Turntable_NEUTRAL = atof(parameter);
+ else{ *Turntable_NEUTRAL = TurnTable_NEUTRAL;
SDerrorcount++;
}
- if(GetParameter(fp,paramNames[11],parameter)) *MatchSpeed = atof(parameter);
- else{ *MatchSpeed = MATCH_SPEED;
+ if(GetParameter(fp,paramNames[9],parameter)) *Matchspeed = atof(parameter);
+ else{ *Matchspeed = MatchSpeed;
SDerrorcount++;
}
- if(GetParameter(fp,paramNames[12],parameter)) *minFocus = atof(parameter);
- else{ *minFocus = MINFOCUS;
+ if(GetParameter(fp,paramNames[10],parameter)) *Minfocus = atof(parameter);
+ else{ *Minfocus = minFocus;
SDerrorcount++;
}
@@ -597,33 +679,34 @@
}else{ //ファイルがなかったら
pc.printf("fp was null.\r\n");
- *servo_NEUTRAL_R = SERVO_NEUTRAL_R;
- *servo_NEUTRAL_L = SERVO_NEUTRAL_L;
- *servo_FORWARD_R = SERVO_FORWARD_R;
- *servo_FORWARD_L = SERVO_FORWARD_L;
- *servo_back_R = SERVO_BACK_R;
- *servo_back_L = SERVO_BACK_L;
- *servo_slow_FORWARD_R = SERVO_SLOW_FORWARD_R;
- *servo_slow_FORWARD_L = SERVO_SLOW_FORWARD_L;
- *TurnTable_NEUTRAL = TURNTABLE_NEUTRAL;
- *MatchSpeed = MATCH_SPEED;
- *minFocus = MINFOCUS;
+ *Servo_NEUTRAL_R = servo_NEUTRAL_R;
+ *Servo_NEUTRAL_L = servo_NEUTRAL_L;
+ *Servo_FORWARD_R = servo_FORWARD_R;
+ *Servo_FORWARD_L = servo_FORWARD_L;
+ *Servo_back_R = servo_back_R;
+ *Servo_back_L = servo_back_L;
+ *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
+ *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
+ *Turntable_NEUTRAL = TurnTable_NEUTRAL;
+ *Matchspeed = MatchSpeed;
+ *Minfocus = minFocus;
SDerrorcount = -1;
}
pc.printf("SDsetup finished.\r\n");
if(SDerrorcount == 0) pc.printf("setting option is success\r\n");
else if(SDerrorcount == -1) pc.printf("ERROR 1. cannot open option\r\n");
else if(SDerrorcount > 0) pc.printf("ERROR 2. reading parameter is failed[%d]\r\n",SDerrorcount);
-
+ /*
pc.printf("SERVO_NEUTRAL_R = %f, SERVO_NEUTRAL_L = %f\r\n", *g_kpRUD, *g_kiRUD);
pc.printf("SERVO_FORWARD_R = %f, SERVO_FORWARD_L = %f, kdRUD = %f\r\n", *servo_FORWARD_R, *servo_FORWARD_L);
pc.printf("SERVO_BACK_R = %f, SERVO_BACK_L = %f\r\n", *servo_back_R, *servo_back_L);
pc.printf("SERVO_SLOW_FORWARD_R = %f, SERVO_SLOW_FORWARD_L = %f\r\n", *servo_slow_FORWARD_R, *servo_slow_FORWARD_L);
pc.printf("TURNTABLE_NEUTRAL = %f, MATCH_SPEED = %f\, MINFOCUS = %fr\n", *TurnTable_NEUTRAL, *MatchSpeed,*minFocus);
-
+ */
return SDerrorcount;
}
+
int GetParameter(FILE *fp, const char *paramName,char parameter[]){
int i=0, j=0;
int strmax = 200;