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;