ショウのマイクロマウス研修
DCマウス研修STM32マイクロマウス

ショウのマイクロマウス制作-Part29

ショウのマイクロマウス研修

こんにちはショウです。今回はSPIを使ってIMUとエンコーダの確認をしていきます。

エンコーダ基板は最初の基板では取り付けていませんが、修正基板で確認したので、ここで説明します。

Part13をベースに作成しています。

IMUはICM42688-P、エンコーダはMA702を使用しています。

CubeMX

IMUとエンコーダを別々のSPIピンに接続しているので、2つSPIの設定をCubeMXでしていきます。

SPIはICによって通信パターンが違うものが存在し、通信時のピン出力が逆になるものがあります。

そのため、使うIC毎にSPIのラインを分けておくと仕様の違うICを繋いだ時に設定が簡単になるため、分けて作成しています。

今回はSPI1にIMU、SPI2にエンコーダを接続しています。結果的には今回はどちらのICも同じ設定を使うことができたので、CubeMXの設定は同じものを使用しています。通信速度はIMUの最大周波数の24MHz以下になるように設定しています。

IMUとエンコーダはDMAを使って取得していますが、実際にはタイマー割り込み毎にDMAの設定をし直しているので、もう少し自動的に取得できる方法を探して修正したいと思っています。

Part13のSPI関数ではwhile文を使って転送完了を待っていましたが、while文の間処理が止まってしまい、CPUの処理を使わずにデータを転送できるDMAの機能が活かせていないと思ったので、送信と受信完了の割り込みを使ってDMAのチャンネルを停止させるようにしています。

void DMA1_Channel1_Init(void)
{
	LL_DMA_EnableIT_TC(DMA1,LL_DMA_CHANNEL_1);
	LL_DMA_EnableIT_TE(DMA1,LL_DMA_CHANNEL_1);
	LL_DMA_DisableChannel(DMA1,LL_DMA_CHANNEL_1);
}

void DMA1_Channel2_Init(void)
{
	LL_DMA_EnableIT_TC(DMA1,LL_DMA_CHANNEL_2);
	LL_DMA_EnableIT_TE(DMA1,LL_DMA_CHANNEL_2);
	LL_DMA_DisableChannel(DMA1,LL_DMA_CHANNEL_2);
}

void DMA1_Channel3_Init(void)
{
	LL_DMA_EnableIT_TC(DMA1,LL_DMA_CHANNEL_3);
	LL_DMA_EnableIT_TE(DMA1,LL_DMA_CHANNEL_3);
	LL_DMA_DisableChannel(DMA1,LL_DMA_CHANNEL_3);
}

void SPI1_DMA_Communication(uint8_t length)
{
	LL_GPIO_ResetOutputPin(CS_gyro_GPIO_Port, CS_gyro_Pin);

	if( LL_SPI_IsActiveFlag_RXNE(SPI1) == SET)LL_SPI_ReceiveData8(SPI1);
	if( LL_SPI_IsEnabled(SPI1) == RESET) LL_SPI_Enable(SPI1);

	LL_DMA_ConfigAddresses(DMA1,LL_DMA_CHANNEL_2,(uint32_t)SPI1TransmitData,
							LL_SPI_DMA_GetRegAddr(SPI1),LL_DMA_DIRECTION_MEMORY_TO_PERIPH);

	LL_DMA_SetDataLength(DMA1,LL_DMA_CHANNEL_2,length);

	LL_DMA_ConfigAddresses(DMA1,LL_DMA_CHANNEL_1,LL_SPI_DMA_GetRegAddr(SPI1),
							(uint32_t)SPI1ReciveData,LL_DMA_DIRECTION_PERIPH_TO_MEMORY);

	LL_DMA_SetDataLength(DMA1,LL_DMA_CHANNEL_1,length);

	LL_DMA_EnableChannel(DMA1,LL_DMA_CHANNEL_2);
	LL_DMA_EnableChannel(DMA1,LL_DMA_CHANNEL_1);
/*
	while(tx_flag != 1);
	tx_flag = 0;
	LL_DMA_DisableChannel(DMA1,LL_DMA_CHANNEL_2);
	while(rx_flag != 1);
	rx_flag = 0;
	LL_DMA_DisableChannel(DMA1,LL_DMA_CHANNEL_1);
	LL_GPIO_SetOutputPin(GPIOx,CS_Pin);
*/
}

void SPI1_DMA1_ReceiveComplete_Callback(void)
{
	LL_DMA_DisableChannel(DMA1,LL_DMA_CHANNEL_1);
	LL_GPIO_SetOutputPin(CS_gyro_GPIO_Port, CS_gyro_Pin);
//	rx_flag = 1;
}

void SPI1_DMA1_TransmitComplete_Callback(void)
{
	LL_DMA_DisableChannel(DMA1,LL_DMA_CHANNEL_2);
//	tx_flag = 1;
}

uint8_t Get_SPI1ReciveData(uint8_t num){
	return SPI1ReciveData[num];
}

void SetSPI1TransmitData(uint8_t num, uint8_t data){
	SPI1TransmitData[num] = data;
}

IMU

IMUは機体角度を取得するためにz軸のデータを取得できるように設定しています。ICM42688-Pではz軸のデータは0x29と0x2Aに保存されているので、DMAを使って一度に2byteのデータを1ms毎にタイマー割り込みを使って取得しています。

IMUで取得した角速度と割り込み時間を使って角度を計算します。

IMUにはドリフトがあるので、静止状態でも出力は0にならないので、対策としてリファレンス関数を作成して、取得した値から引くことでドリフト分の値をキャンセルします。(ししかわさんのブログと同じ方法です。)リファレンス関数は機体が安定しているタイミングを見計らって取得させます。タイミングとしては走行直前などが機体が安定していて取得しやすいです。

リファレンス関数は単純にIMUの値を複数回取得して平均を取っています。

void ICM_42688_WriteByte(uint8_t reg,uint8_t data)
{
	SetSPI1TransmitData(0, reg);
	SetSPI1TransmitData(1, data);

	SPI1_DMA_Communication(2);
}

void ICM_42688_ReadByte(uint8_t reg)
{
	SetSPI1TransmitData(0,(reg | 0x80));
	SetSPI1TransmitData(1,0x00);
	SetSPI1TransmitData(2,0x00);

	SPI1_DMA_Communication(3);
}


void ICM_42688_init(void)
{
	uint8_t reg78 = 0x4E;	//pwr_mgmt0

	ICM_42688_WriteByte(reg78,0x0F);
	LL_mDelay(1);
}

void ICM_42688_GyroRead_DMA(uint8_t reg) //reg 29 2A
{
	ICM_42688_ReadByte(reg);
}

void ICM_42688_GyroData(void)
{
	s_GyroVal=((uint16_t)Get_SPI1ReciveData(1)<<8|Get_SPI1ReciveData(2));
}

void GYRO_SetRef( void )
{
	uint16_t i;
	uint32_t ul_ref = 0;

	for( i=0; i<GYRO_REF_NUM; i++){
		ul_ref += (uint32_t)s_GyroVal;
		LL_mDelay(1);
	}

	l_GyroRef = (ul_ref * 100) / GYRO_REF_NUM ;
}

エンコーダ

エンコーダに使っているMA702は少し特殊なICで、普通ならデータを保存しているレジスタにアクセスしてデータを読み出すのですが、レジスタを指定しないでデータを転送するとエンコーダの値が帰ってくるという形式になっています。

そのため、初期設定のアドレスの指示以外は0をクロックに合わせて転送するだけでエンコーダの結果を取得できます。

エンコーダは左右2つあるので、CSのGPIOと連動させてどちらのICを読みこむのか指示しています。

void MA702_ReadByte(en_endir dir)
{
	encoderdir = dir;
	SetSPI2TransmitData(0,0x00);
	SetSPI2TransmitData(1,0x00);

	SPI2_DMA_Communication(2,encoderdir);
}

void recv_spi_encoder(en_endir dir)
{
	MA702_ReadByte(dir);
}

void Set_encoder_data(en_endir dir)
{
	if(dir == enL) ENC_L_CNT = ((uint16_t)Get_SPI2ReciveData(0)<<8|Get_SPI2ReciveData(1));
	else ENC_R_CNT = ((uint16_t)Get_SPI2ReciveData(0)<<8|Get_SPI2ReciveData(1));
}

取得タイミングは壁センサの時と同じように4kHzで割り込みをさせて、その中で、データを転送した次の割り込みで取得したデータを変数に保存させています。

DMAの転送先と使い方を変えるとこんなことしなくて済む気はするので、ここも修正していこうと思います。

void TIM4_IRQHandler(void)
{
  static uint8_t i = 0;
　if ( LL_TIM_IsActiveFlag_UPDATE(TIM4)==1){
　    LL_TIM_ClearFlag_UPDATE(TIM4);
　}
  switch(i){
	case 0:
		recv_spi_encoder(enL);
		break;
	case 1:
		Set_encoder_data(enL);
      　　　　　recv_spi_encoder(enR);
		break;
	case 2:
      　　　　　Set_encoder_data(enR);
      　　　　　ICM_42688_GyroRead_DMA(0x29);
		break;
	case 3:
		ICM_42688_GyroData();
		GYRO_Pol();
		break;
	}
	i = (i+1)%4;
}

今回までで機能確認が終わったので次からは走行プログラムを作成していきます。

タイトルとURLをコピーしました