航空研究会
/
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
Revision 42:eadf30b9ecd2, committed 2020-01-07
- Comitter:
- imanomadao
- Date:
- Tue Jan 07 11:29:44 2020 +0000
- Parent:
- 41:d142afd3c2f7
- Commit message:
- a
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Mar 07 17:50:58 2019 +0000
+++ b/main.cpp Tue Jan 07 11:29:44 2020 +0000
@@ -5,1100 +5,18 @@
#include "SkipperSv2.h"
#include "falfalla.h"
-//MPU_check用
-#define PI 3.14159265358979
-
-#define servo_NEUTRAL_R 1614
-#define servo_NEUTRAL_L 1621
-#define servo_slow_FORWARD_R 1560
-#define servo_slow_FORWARD_L 1560
-#define servo_high_FORWARD_R 1860
-#define servo_high_FORWARD_L 1860
-#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
-#define turntable_speed 1800
-#define time360 110
-#define match_wid 5
-#define camera_board_wait 100
-
-#define ReturnCount 4
-
-#define MOVE_NEUTRAL 0
-#define MOVE_FORWARD 1
-#define MOVE_LEFT 2
-#define MOVE_RIGHT 3
-#define MOVE_BACK 4
-#define GOAL_FORWARD 5 //ゴール付近_ゆっくり
-#define GOAL_LEFT 6
-#define GOAL_RIGHT 7
-#define MAX_FORWARD 8 //はやい_姿勢修正用
-#define MAX_BACK 9
-
-void getSF_Serial_jevois();
-void getSF_Serial_pi();
-
-//MPU_check用
-void SensingMPU();
-
-void MoveCansat(char g_landingcommand);
-void setup();
-void Init_sensors();
-void DisplayClock();
-void DebugPrint();
-void SensingHMC();
-void MoveCameraBoard();
-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_high_FORWARD_R,int *Servo_high_FORWARD_L,
- 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 *Turntable_speed,
- int *Time360, int *Match_wid,
- int *Camera_board_wait
- );
-
-static float nowAngle[3] = {0,0,0};//,nowAngle_HMC=0;
-float FirstROLL = 0.0, FirstPITCH = 0.0 ,FirstYAW = 0.0,g_FirstYAW_HMC;
-
-bool setupFlag = false;
-bool CameraDegFlag = false;
-bool jevoisFlag = true;
-bool FocusFlag = false;
-
-int g_CameraDegCounter = 0; //カメラの回転数をカウント
-
-enum Angle{ROLL, PITCH, YAW}; //yaw:北を0とした絶対角度
-
-Timer t;
-Timer t2;
-
-FILE *g_fp;
-
-SDFileSystem sd(PB_15, PB_14, PB_13, PB_12, "sd");
-
-RawSerial pc(PA_2,PA_3,115200); //uart2 jevoiis
-//pa2:UART2_TX,pa3:UART2_RX
-RawSerial pc2(PA_11,PA_12,115200); //uart1 raspberry
-//pb6:UART1_TX,pb7:UART1_RX
-
-//PWM pin宣言
-PwmOut servoR(PC_6); //TIM3_CH1 車輪右
-PwmOut servoL(PC_7); //TIM3_CH2 車輪左
-PwmOut servoTurnTable(PB_0); //TIM3_CH3 カメラ台回転Servo
-PwmOut servoCameradeg(PB_1); //TIM3_CH4 カメラ角度調節Servo
-PwmOut servoCameraPinto(PB_6); //TIM4_CH1 カメラピント合わせ
-PwmOut servoCameramount(PC_8); //skipperウラ カメラマウント起動
-//PwmOut servoGetUP(PC_8); //skipperウラ 起き上がり動作
+RawSerial pc(USBTX,USBRX,115200); //pa2:UART2_TX,pa3:UART2_RX
-/*通信用のpinは
- PA_3(UART2_Rx) skipperウラ
- PA_12(UART6_Rx) skipperオモテ USB端子より
- PB_7 (UART1_Rx) skipperオモテ 6番目 TIM4_CH2
-*/
-
-/*超音波はRaspberryPiに積む*/
-
-DigitalOut led1(PA_0); //tanakaOK kimuraは動作不良
-DigitalOut led2(PA_1); //tanakaOK kimuraOK
-DigitalIn BoardCheck(PB_8);
-DigitalOut led4(PB_4); //使ってないよ
-
-
-//外付けコンパス
-//HMC5883L compass(PB_9, PB_8); //コンパスセンサー TIM4_CH3とCH4
-
-char g_landingcommand='N';
-
-//MPU_check用
-MPU6050DMP6 mpu6050(PC_0,&pc);
-
-
-int main() {
- BoardCheck.mode(PullDown);
- //MPU_check
- setup();
- //コンパスチェック用
- /*while(1){
- //SensingMPU();
- SensingHMC();
- //pc.printf("%3.2f\t",nowAngle[2]);
- // pc.printf("%3.2f\t",nowAngle_HMC); //HMC
- pc.printf("\r\n");
- wait_ms(30);
- }*/
-
-
- // シリアル通信受信の割り込みイベント登録
- pc.attach(getSF_Serial_jevois, Serial::RxIrq);
- pc2.attach(getSF_Serial_pi, Serial::RxIrq);
- t2.start();
- /*
- NVIC_DisableIRQ(USART2_IRQn);
- while(g_landingcommand != 'B'){
- wait_ms(30);
- }
- NVIC_EnableIRQ(USART2_IRQn);
- jevoisFlag = true;
- g_landingcommand='N';
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Parachute cut.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- wait(5);
- MoveCansat('1');
- servoR.pulsewidth_us(Servo_NEUTRAL_R);
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- wait(10);
- //pc.printf("start\r\n");
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Start sarching target .\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- */
-
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
- //pc.printf("MatchPosition\r\n");
- while(BoardCheck == 0){
- }
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
-
- servoCameraPinto.pulsewidth_us(Focus_NEUTRAL-200);
- wait(1);
- servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
-
- while(1) {
- if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
- else NVIC_DisableIRQ(USART2_IRQn);
- //pc.printf("Move Camera Board\r\n");
- MoveCameraBoard();
- if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
- else NVIC_DisableIRQ(USART2_IRQn);
-
- //pc.printf("Position\r\n");
- //pc.printf("g_landingcommand=%c\r\n",g_landingcommand);
-
- NVIC_DisableIRQ(USART6_IRQn);
- NVIC_DisableIRQ(USART2_IRQn);
- if(g_landingcommand!='N') MatchPosition();
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
-
- if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
- else NVIC_DisableIRQ(USART2_IRQn);
-
- //pc.printf("Move Cansat\r\n");
- MoveCansat(g_landingcommand);
- if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
- else NVIC_DisableIRQ(USART2_IRQn);
- wait_ms(23);
-
- //pc.printf("finish\r\n");
- if(jevoisFlag==true) NVIC_EnableIRQ(USART6_IRQn);
- else NVIC_EnableIRQ(USART2_IRQn);
- }
-}
-
+PwmOut brushless_R(PC_6);
-void MoveCansat(char a)
-{
- NVIC_DisableIRQ(USART6_IRQn);
- NVIC_DisableIRQ(USART2_IRQn);
- switch(a){
- case 'N': //MOVE_NEUTRAL
- servoR.pulsewidth_us(Servo_NEUTRAL_R);
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
- fprintf(g_fp, "Time = %d min %d sec : Goal is not found.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'Y': //MOVE_FORWARD
- servoR.pulsewidth_us(Servo_high_FORWARD_R);
- servoL.pulsewidth_us(Servo_high_FORWARD_L);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move forward .\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'l': //MOVE_LEFT Low Speed
- servoR.pulsewidth_us(Servo_slow_FORWARD_R);
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move left low speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'L': //MOVE_LEFT High Speed
- servoR.pulsewidth_us(Servo_high_FORWARD_R);
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move left high speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'r': //MOVE_RIGHT Low Speed
- servoR.pulsewidth_us(Servo_NEUTRAL_R);
- servoL.pulsewidth_us(Servo_slow_FORWARD_L);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move right low speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'R': //MOVE_RIGHT High Speed
- servoR.pulsewidth_us(Servo_NEUTRAL_R);
- servoL.pulsewidth_us(Servo_high_FORWARD_L);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move left high speed.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'G': //GOAL_FORWARD
- servoR.pulsewidth_us(Servo_slow_FORWARD_R);
- servoL.pulsewidth_us(Servo_slow_FORWARD_L);
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move goal forward mode.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case '0': //MOVE_FORWARD Speed Level 1
- servoR.pulsewidth_us(Servo_high_FORWARD_R);
- servoL.pulsewidth_us(Servo_high_FORWARD_L);
- wait(5);
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Finish!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- servoR.pulsewidth_us(Servo_NEUTRAL_R);
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- exit(0);
- NVIC_EnableIRQ(USART6_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);
- wait(5);
- /*do{
- SensingMPU();
- wait_ms(30);
- }while((nowAngle[PITCH]>20||nowAngle[PITCH]<-20)&&(nowAngle[ROLL]>20||nowAngle[ROLL]<-20)); */
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 1sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- 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);
- wait(10);
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 2sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- 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);
- //Camera_board_wait =
- wait(30); //6m
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 3sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- 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);
- wait(50);//10m
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 4sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case '5': //MOVE_FORWARD Speed Level 5
- servoR.pulsewidth_us(Servo_high_FORWARD_R);
- servoL.pulsewidth_us(Servo_high_FORWARD_L);
- wait(100);//20m
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move forward 5sec!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'M': //MatchPosition
- servoR.pulsewidth_us(Matchspeed);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'P': //jevoisからraspberry piへの切り替え
- jevoisFlag = false;
- servoR.pulsewidth_us(Servo_high_FORWARD_R);
- servoL.pulsewidth_us(Servo_high_FORWARD_L);
- //Camera_board_wait =
- wait(15);
- servoR.pulsewidth_us(Servo_NEUTRAL_R);
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
- wait_ms((float)Time360/(float)2);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- case 'B':
- /*RasPiからの超音波判定(プログラムスタート部)*/
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- default :
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- break;
-
- }
-
- return;
-}
-
-void MoveCameraBoard(){
- if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
- else NVIC_DisableIRQ(USART2_IRQn);
- //pc.printf("start\r\n");
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- MoveCansat('N');
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
- else NVIC_DisableIRQ(USART2_IRQn);
- //pc.printf("ok\r\n");
- g_landingcommand='N';
- servoCameradeg.pulsewidth_us(Camera_deg_B);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- wait_ms(30);
- if(g_CameraDegCounter==0){
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
- wait_ms(Camera_board_wait*ReturnCount);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
-
-
- for(int i=200; i<Camera_deg_B-Camera_deg_A;i=i+200){
-
- servoCameradeg.pulsewidth_us(Camera_deg_B-i);
- servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- wait_ms(30);
- //pc.printf("zoom2\r\n");
- if(jevoisFlag == true) FocusAdjust();
- else wait(1);
-
- if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
- else NVIC_DisableIRQ(USART2_IRQn);
-
- if(g_landingcommand!='N') return;
-
- }
-
- }
-
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
- wait_ms(Camera_board_wait);
- g_CameraDegCounter++;
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- //pc.printf("zoom1\r\n");
- if(jevoisFlag == true) FocusAdjust();
- else wait(1);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
-
- if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
- else NVIC_DisableIRQ(USART2_IRQn);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
-
-
- if(g_landingcommand!='N') return;
-
- for(int i=200; i<Camera_deg_B-Camera_deg_A;i=i+200){
-
- servoCameradeg.pulsewidth_us(Camera_deg_B-i);
- servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- wait_ms(30);
- //pc.printf("zoom2\r\n");
- if(jevoisFlag == true) FocusAdjust();
- else wait(1);
-
- if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
- else NVIC_DisableIRQ(USART2_IRQn);
-
- if(g_landingcommand!='N') return;
-
- }
-
-
- //pc.printf("g_CameraDegCounter = %d\r\n",g_CameraDegCounter);
-
- //ターゲットを発見できなかった場合
- if(g_CameraDegCounter-ReturnCount > (float)Time360/(float)Camera_board_wait){
-
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
- wait(1);
- while(BoardCheck == 0){
- }
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
-
- if(jevoisFlag == true ) {
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
- wait_ms((float)Time360/(float)2);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- }
- if(jevoisFlag == false) MoveCansat('4');
-
- jevoisFlag = !jevoisFlag;
-
- g_CameraDegCounter = 0;
- }
-
- //pc.printf("Move Board Finish\r\n");
- return;
-}
-
-void MatchPosition(){
- NVIC_DisableIRQ(USART6_IRQn);
- NVIC_DisableIRQ(USART2_IRQn);
-
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Get signal!.\r\n",(int)t2.read()/60,(int)t2.read()%60);
- fclose(g_fp);
-
- int TurnTime ;
-
- TurnTime = g_CameraDegCounter - ReturnCount;
-
- if(TurnTime >=0){
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
- //wait_ms(Camera_board_wait*TurnTime);
- //pc.printf("MatchPosition\r\n");
- while(BoardCheck == 0){
- }
- }else{
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL - Turntable_speed);
- //wait_ms(-Camera_board_wait*TurnTime);
- //pc.printf("MatchPosition\r\n");
- while(BoardCheck == 0){
- }
- }
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
-
- if(jevoisFlag == false ){
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL + Turntable_speed);
- wait_ms((float)Time360/(float)2);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
+int main(){
+ int a_=500;
+ while(1){
+ for (int i=0;i<50;i++){
+ brushless_R.pulsewidth_us(a_);
+ a_ += 10;
+ pc.printf("a=%d\r\n",a_);
+ wait(0.25);
}
-
- int SetLoop=0;
-
- while(SetLoop<30){
- SensingMPU();
- wait_ms(10);
- //SensingHMC();
- //wait_ms(20);
- //DebugPrint();
- SetLoop++;
- //pc.printf("nowAngle_HMC=%f\tMPU=%f\r\n",nowAngle_HMC,nowAngle[YAW]);
-
- }
-
- static float TargetDeg = 0;
-
- TargetDeg = 360*(float)Camera_board_wait*((float)g_CameraDegCounter - ReturnCount)/(float)Time360;
-
- if(TargetDeg > 360)TargetDeg = TargetDeg - 360;
- if(TargetDeg < 0)TargetDeg = TargetDeg + 360;
-
- float HighTargetYaw = nowAngle[YAW] + TargetDeg + Match_wid;
- float LowTargetYaw = nowAngle[YAW] + TargetDeg - Match_wid;
-
-
- if(HighTargetYaw >= 360.0f) HighTargetYaw = HighTargetYaw - 360.0f;
- if(LowTargetYaw >= 360.0f) LowTargetYaw = LowTargetYaw - 360.0f;
- if(LowTargetYaw < 0.0f) LowTargetYaw = LowTargetYaw + 360.0f;
-
- //pc.printf("\r\nnow=%f\t,high=%f\t,low=%f\r\n",nowAngle[YAW],HighTargetYaw,LowTargetYaw);
-
- MoveCansat('r');
-
- if(HighTargetYaw-LowTargetYaw<0){
- while(nowAngle[YAW] > HighTargetYaw && nowAngle[YAW] < LowTargetYaw){
- //MoveCansat('r');
- SensingMPU();
- wait_ms(10);
- //pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
- }
- }else{
- while(nowAngle[YAW] > HighTargetYaw || nowAngle[YAW] < LowTargetYaw){
- //MoveCansat('r');
- SensingMPU();
- wait_ms(10);
- //pc.printf("nowAngle_MPU=%f\r\n",nowAngle[YAW]);
- }
- }
- g_CameraDegCounter = 0;
-
- g_fp = fopen( "/sd/Datalog01.txt","a" );
- fprintf(g_fp, "Time = %d min %d sec : Cansat move %fdeg.\r\n",(int)t2.read()/60,(int)t2.read()%60,TargetDeg);
- fclose(g_fp);
-
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- return;
-}
-
-void FocusAdjust(){
- if(jevoisFlag==true) NVIC_DisableIRQ(USART6_IRQn);
- else NVIC_DisableIRQ(USART2_IRQn);
-
- if(FocusFlag == false){
- servoCameraPinto.pulsewidth_us(Focus_NEUTRAL + Pint_speed);
- servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- wait(Pint_wait);
- FocusFlag = !FocusFlag;
- }else{
- servoCameraPinto.pulsewidth_us(Focus_NEUTRAL - Pint_speed);
- servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- wait(Pint_wait);
- FocusFlag = !FocusFlag;
- }
- servoCameraPinto.pulsewidth_us(Focus_NEUTRAL);
-
- return;
-}
-
-void getSF_Serial_jevois(){
-
- //pc.printf("jevois\r\n");
-
-
- static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
-
- static int bufcounter=0;
-
-
-
- if(pc.readable()) { // 受信確認
-
- SFbuf[bufcounter] = pc.getc(); // 1文字取り出し
- if(SFbuf[0]!='S'){
- //pc.printf("x");
- return;
- }
-
-
-
- //pc.printf("%c",SFbuf[bufcounter]);
-
- if(SFbuf[0]=='S' && bufcounter<5)bufcounter++;
-
- if(bufcounter==5 && SFbuf[4]=='F'){
-
- g_landingcommand = SFbuf[1];
- wait_ms(31);//信号が速すぎることによる割り込み防止
- //pc.printf("%c",g_landingcommand);
- //wait_ms(20);
- //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
- bufcounter = 0;
- memset(SFbuf, 0, sizeof(SFbuf));
- NVIC_ClearPendingIRQ(USART2_IRQn);
- //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
- }
-
- else if(bufcounter>=5){
- //pc.printf("Communication Falsed.\r\n");
- memset(SFbuf, 0, sizeof(SFbuf));
- bufcounter = 0;
- NVIC_ClearPendingIRQ(USART2_IRQn);
- }
- }
-
-
-}
-
-
-void getSF_Serial_pi(){
-
- //NVIC_DisableIRQ(USART2_IRQn);
-
- static char SFbuf[16]={'Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q','Q'};
-
- static int bufcounter=0;
-
-
-
- if(pc2.readable()) { // 受信確認
-
- SFbuf[bufcounter] = pc2.getc(); // 1文字取り出し
- if(SFbuf[0]!='S'){
- //pc.printf("x");
- return;
- }
-
-
-
- //pc.printf("%c",SFbuf[bufcounter]);
-
- if(SFbuf[0]=='S'&&bufcounter<5)bufcounter++;
-
- if(bufcounter==5 && SFbuf[4]=='F'){
-
- g_landingcommand = SFbuf[1];
- wait_ms(31);//信号が速すぎることによる割り込み防止
- //pc.printf("%c",g_landingcommand);
- //wait_ms(20);
- //if(g_landingcommand=='Y')g_SerialTargetYAW = ConvertByteintoFloat(SFbuf[2], SFbuf[3]);
- bufcounter = 0;
- memset(SFbuf, 0, sizeof(SFbuf));
- NVIC_ClearPendingIRQ(USART2_IRQn);
- //pc.printf("command = %c, commandYAW = %f\r\n", g_landingcommand, g_SerialTargetYAW);
- }
-
- else if(bufcounter>=5){
- //pc.printf("Communication Falsed.\r\n");
- memset(SFbuf, 0, sizeof(SFbuf));
- bufcounter = 0;
- NVIC_ClearPendingIRQ(USART2_IRQn);
- }
- }
-
- //NVIC_EnableIRQ(USART2_IRQn);
-
-}
-
-
-void setup(){
-
- led1 = 1;
- led2 = 1;
- //led3 = 1;
- led4 = 1;
-
- servoR.pulsewidth_us(Servo_NEUTRAL_R); //servo initialize
- servoL.pulsewidth_us(Servo_NEUTRAL_L);
- servoTurnTable.pulsewidth_us(Turntable_NEUTRAL);
- servoCameradeg.pulsewidth_us(Camera_deg_B);
- servoCameraPinto.pulsewidth_us(focus_NEUTRAL);
-
- SetOptions(&Servo_NEUTRAL_R, &Servo_NEUTRAL_L,
- &Servo_high_FORWARD_R, &Servo_high_FORWARD_L,
- &Servo_slow_FORWARD_R,&Servo_slow_FORWARD_L,
- &Turntable_NEUTRAL,&Matchspeed,&Focus_NEUTRAL,
- &Camera_deg_A, &Camera_deg_B,
- &Pint_speed,&Pint_wait,
- &Turntable_speed,
- &Time360, &Match_wid,
- &Camera_board_wait
- );
-
- Init_sensors();
- //switch2.rise(ResetTrim);
-
- //NVIC_SetPriority(USART6_IRQn,0);
- //NVIC_SetPriority(EXTI0_IRQn,1);
- //NVIC_SetPriority(TIM5_IRQn,2);
- //NVIC_SetPriority(EXTI9_5_IRQn,3);
- //NVIC_SetPriority(USART2_IRQn,4);
-
- NVIC_SetPriority(USART6_IRQn,0);//割り込み優先度
- NVIC_SetPriority(USART2_IRQn,1);
-
- DisplayClock();
- t.start();
-
- pc.printf("MPU calibration start\r\n");
- //pc.printf("HMC calibration start\r\n");
-
- float offsetstart = t.read();
- while(t.read() - offsetstart < 26){
- SensingMPU();
- //SensingHMC();
- for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]);
- //pc.printf("\t%3.2f\t",nowAngle_HMC);
- pc.printf("\r\n");
- led1 = !led1;
- led2 = !led2;
- //led3 = !led3;
- led4 = !led4;
- }
-
- FirstROLL = nowAngle[ROLL];
- FirstPITCH = nowAngle[PITCH];
- nowAngle[ROLL] -=FirstROLL;
- nowAngle[PITCH] -=FirstPITCH;
- FirstYAW = nowAngle[YAW];
- nowAngle[YAW] -= FirstYAW;
-
- //g_FirstYAW_HMC = nowAngle_HMC;
- //nowAngle_HMC -=g_FirstYAW_HMC;
- //if(nowAngle_HMC<0)nowAngle_HMC+=360;
-
- led1 = 0;
- led2 = 0;
- //led3 = 0;
- led4 = 0;
-
- wait(0.2);
-
-
- g_fp = fopen( "/sd/Datalog01.txt" ,"a" );
- if( g_fp == NULL ) {
- pc.printf( "File open error!!!\r\n" );
- }
- fprintf(g_fp,"\r\n-------------------------\r\n");
- fprintf(g_fp,"All initialized.\r\n");
- fclose( g_fp );
-
-
- pc.printf("All initialized\r\n");
- setupFlag=true;
+ }
}
-
-
-void SensingMPU(){
- //static int16_t deltaT = 0, t_start = 0;
- //t_start = t.read_us();
-
- float rpy[3] = {0};
- static uint16_t count_changeRPY = 0;
- static bool flg_checkoutlier = false;
- NVIC_DisableIRQ(USART6_IRQn);
- NVIC_DisableIRQ(USART2_IRQn);
- NVIC_DisableIRQ(TIM5_IRQn);
- NVIC_DisableIRQ(EXTI0_IRQn);
- NVIC_DisableIRQ(EXTI9_5_IRQn);
-
- mpu6050.getRollPitchYaw_Skipper(rpy);
-
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- NVIC_EnableIRQ(TIM5_IRQn);
- NVIC_EnableIRQ(EXTI0_IRQn);
- NVIC_EnableIRQ(EXTI9_5_IRQn);
-
-
- //外れ値対策
- for(uint8_t i=0; i<3; i++) rpy[i] *= 180.0f/(float)PI;
- rpy[ROLL] -= FirstROLL;
- rpy[PITCH] -= FirstPITCH;
- if(!setupFlag){
- rpy[YAW] -= FirstYAW;
- }else{
- if(rpy[YAW] >= FirstYAW){
- rpy[YAW] -= FirstYAW;
- }else{
- rpy[YAW] += 360.0f;
- rpy[YAW] -= FirstYAW;
- }
- }
-
- for(uint8_t i=0; i<3; i++) {if(rpy[i] < nowAngle[i]-10 || rpy[i] > nowAngle[i]+10) {flg_checkoutlier = true;}}
- if(!flg_checkoutlier || count_changeRPY >= 2){
- for(uint8_t i=0; i<3; i++){
- if(rpy[i] > nowAngle[i]-180 && rpy[i] < nowAngle[i]+180){
- nowAngle[i] = (rpy[i] + nowAngle[i])/2.0f; //2つの移動平均
- }else{
- nowAngle[i] = rpy[i];
- }
- }
- count_changeRPY = 0;
- }else count_changeRPY++;
- flg_checkoutlier = false;
-
-}
-
-
-void Init_sensors(){
- if(mpu6050.setup() == -1){
- pc.printf("failed initialize\r\n");
- }
-}
-
-
-void DisplayClock(){
- pc.printf("System Clock = %d[MHz]\r\n", HAL_RCC_GetSysClockFreq()/1000000);
- pc.printf("HCLK Clock = %d[MHz]\r\n", HAL_RCC_GetHCLKFreq()/1000000);
- pc.printf("PCLK1 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK1Freq()/1000000);
- pc.printf("PCLK2 Clock = %d[MHz]\r\n", HAL_RCC_GetPCLK2Freq()/1000000);
- pc.printf("\r\n");
-}
-
-/*void SensingHMC(){
- //static int16_t deltaT = 0, t_start = 0;
- //t_start = t.read_us();
-
- float rpy=0;
- static uint16_t count_changeRPY = 0;
- static bool flg_checkoutlier = false;
- NVIC_DisableIRQ(USART6_IRQn);
- NVIC_DisableIRQ(USART2_IRQn);
- //NVIC_DisableIRQ(TIM5_IRQn);
- //NVIC_DisableIRQ(EXTI0_IRQn);
- //NVIC_DisableIRQ(EXTI9_5_IRQn);
-
- rpy= compass.getHeadingXYDeg(Time360,Match_wid);
-
- NVIC_EnableIRQ(USART6_IRQn);
- NVIC_EnableIRQ(USART2_IRQn);
- //NVIC_EnableIRQ(TIM5_IRQn);
- //NVIC_EnableIRQ(EXTI0_IRQn);
- //NVIC_EnableIRQ(EXTI9_5_IRQn);
-
-
- //外れ値対策
- //rpy*= 180.0f/PI;
- if(!setupFlag){
- rpy -= g_FirstYAW_HMC;
- }else{
- if(rpy >= g_FirstYAW_HMC){
- rpy -= g_FirstYAW_HMC;
- }else{
- rpy += 360.0f;
- rpy -= g_FirstYAW_HMC;
- }
- }
-
- if(rpy < nowAngle_HMC-10 || rpy > nowAngle_HMC+10) {flg_checkoutlier = true;}
- if(!flg_checkoutlier || count_changeRPY >= 2){
-
- nowAngle_HMC = (rpy + nowAngle_HMC)/2.0f; //2つの移動平均
-
- count_changeRPY = 0;
- }else count_changeRPY++;
- flg_checkoutlier = false;
-
-}*/
-
-int SetOptions(int *Servo_NEUTRAL_R, int *Servo_NEUTRAL_L,
- int *Servo_high_FORWARD_R, int *Servo_high_FORWARD_L,
- 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 *Turntable_speed,
- int *Time360, int *Match_wid,
- int *Camera_board_wait
- ){
-
- pc.printf("SDsetup start.\r\n");
-
- FILE *fp;
- char parameter[20]; //文字列渡す用の配列
- int SDerrorcount = 0; //取得できなかった数を返す
- const char *paramNames[] = {
- "SERVO_NEUTRAL_R",
- "SERVO_NEUTRAL_L",
- "SERVO_HIGH_FORWARD_R",
- "SERVO_HIGH_FORWARD_L",
- "SERVO_SLOW_FORWARD_R",
- "SERVO_SLOW_FORWARD_L",
- "TURNTABLE_NEUTRAL",
- "MATCH_SPEED",
- "FOCUS_NEUTRAL",
- "CAMERA_DEG_A",
- "CAMERA_DEG_B",
- "PINT_SPEED",
- "PINT_WAIT",
- "TURNTABLE_SPEED",
- "TIME360",
- "MATCH_WID",
- "CAMERA_BOARD_WAIT"
- };
-
- fp = fopen("/sd/option.txt","r");
-
- 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;
- SDerrorcount++;
- }
- 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_high_FORWARD_R = atof(parameter);
- else{ *Servo_high_FORWARD_R = servo_high_FORWARD_R;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[3],parameter)) *Servo_high_FORWARD_L = atof(parameter);
- else{ *Servo_high_FORWARD_L = servo_high_FORWARD_L;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[4],parameter)) *Servo_slow_FORWARD_R = atof(parameter);
- else{ *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[5],parameter)) *Servo_slow_FORWARD_L = atof(parameter);
- else{ *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
- SDerrorcount++;
- }
-
- if(GetParameter(fp,paramNames[6],parameter)) *Turntable_NEUTRAL = atof(parameter);
- else{ *Turntable_NEUTRAL = turntable_NEUTRAL;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[7],parameter)) *Matchspeed = atof(parameter);
- else{ *Matchspeed = matchspeed;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[8],parameter)) *Focus_NEUTRAL = atof(parameter);
- else{ *Focus_NEUTRAL = focus_NEUTRAL;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[9],parameter)) *Camera_deg_A = atof(parameter);
- else{ *Camera_deg_A = camera_deg_A;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[10],parameter)) *Camera_deg_B = atof(parameter);
- else{ *Camera_deg_B = camera_deg_B;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[11],parameter)) *Pint_speed = atof(parameter);
- else{ *Pint_speed = pint_speed;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[12],parameter)) *Pint_wait = atof(parameter);
- else{ *Pint_wait = pint_wait;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[13],parameter)) *Turntable_speed = atof(parameter);
- else{ *Turntable_speed = turntable_speed;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[14],parameter)) *Time360 = atof(parameter);
- else{ *Time360 = time360;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[15],parameter)) *Match_wid = atof(parameter);
- else{ *Match_wid = match_wid;
- SDerrorcount++;
- }
- if(GetParameter(fp,paramNames[16],parameter)) *Camera_board_wait = atof(parameter);
- else{ *Camera_board_wait = camera_board_wait;
- SDerrorcount++;
- }
- fclose(fp);
-
- }else{ //ファイルがなかったら
- pc.printf("fp was null.\r\n");
-
- *Servo_NEUTRAL_R = servo_NEUTRAL_R;
- *Servo_NEUTRAL_L = servo_NEUTRAL_L;
- *Servo_high_FORWARD_R = servo_high_FORWARD_R;
- *Servo_high_FORWARD_L = servo_high_FORWARD_L;
- *Servo_slow_FORWARD_R = servo_slow_FORWARD_R;
- *Servo_slow_FORWARD_L = servo_slow_FORWARD_L;
- *Turntable_NEUTRAL = turntable_NEUTRAL;
- *Matchspeed = matchspeed;
- *Focus_NEUTRAL = focus_NEUTRAL;
- *Camera_deg_A = camera_deg_A;
- *Camera_deg_B = camera_deg_B;
- *Pint_speed = pint_speed;
- *Pint_wait = pint_wait;
- *Turntable_speed = turntable_speed;
- *Time360 = time360;
- *Match_wid = match_wid;
- *Camera_board_wait = camera_board_wait;
- 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 = %d, Servo_NEUTRAL_L = %d\r\n", *Servo_NEUTRAL_R, *Servo_NEUTRAL_L);
- pc.printf("Servo_high_FORWARD_R = %d, Servo_high_FORWARD_L = %d\r\n", *Servo_high_FORWARD_R, *Servo_high_FORWARD_L);
- pc.printf("Servo_slow_FORWARD_R = %d, Servo_slow_FORWARD_L = %d\r\n", *Servo_slow_FORWARD_R, *Servo_slow_FORWARD_L);
- pc.printf("Turntable_NEUTRAL = %d, Matchspeed = %d\r\n", *Turntable_NEUTRAL, *Matchspeed);
- 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);
- pc.printf("Time360 = %d, Match_wid = %d\r\n", *Time360, *Match_wid);
- pc.printf("Camera_board_wait = %d\r\n", *Camera_board_wait);
-
- return SDerrorcount;
-}
-int GetParameter(FILE *fp, const char *paramName,char parameter[]){
- int i=0, j=0;
- int strmax = 200;
- char str[strmax];
-
- rewind(fp); //ファイル位置を先頭に
- while(1){
- if (fgets(str, strmax, fp) == NULL) {
- return 0;
- }
- if (!strncmp(str, paramName, strlen(paramName))) {
- while (str[i++] != '=') {}
- while (str[i] != '\n') {
- parameter[j++] = str[i++];
- }
- parameter[j] = '\0';
- return 1;
- }
- }
-}
-
-
-
-void DebugPrint(){
- //for(uint8_t i=0; i<3; i++) pc.printf("%3.2f\t",nowAngle[i]); //skipper地磁気センサ_デバック用
- //pc.printf("%3.2f\t",nowAngle[2]);
- //pc.printf("%3.2f\t",nowAngle_HMC); //HMC
- //pc.printf("\r\n");
- }
\ No newline at end of file