初期値を調整
Dependencies: mbed MPU6050_2 HMC5883L_2
Revision 12:a2c6207cbce3, committed 2019-02-12
- Comitter:
- taknokolat
- Date:
- Tue Feb 12 02:59:22 2019 +0000
- Parent:
- 11:d44d137831b9
- Commit message:
- a
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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地磁気センサ_デバック用