ESCソースコード
詳細はこちらをご覧ください(YouTube,公式ホームページ)→(準備中)
Compiler:XC8
C version:C90
// PIC18F23K22 Configuration Bit Settings
// 'C' source line config statements
// CONFIG1H #pragma config FOSC = INTIO67 // Oscillator Selection bits (Internal oscillator block) #pragma config PLLCFG = OFF // 4X PLL Enable (Oscillator used directly) #pragma config PRICLKEN = ON // Primary clock enable bit (Primary clock enabled) #pragma config FCMEN = OFF // Fail-Safe Clock Monitor Enable bit (Fail-Safe Clock Monitor disabled) #pragma config IESO = OFF // Internal/External Oscillator Switchover bit (Oscillator Switchover mode disabled)
// CONFIG2L #pragma config PWRTEN = ON // Power-up Timer Enable bit (Power up timer enabled) #pragma config BOREN = SBORDIS // Brown-out Reset Enable bits (Brown-out Reset enabled in hardware only (SBOREN is disabled)) #pragma config BORV = 190 // Brown Out Reset Voltage bits (VBOR set to 1.90 V nominal)
// CONFIG2H #pragma config WDTEN = OFF // Watchdog Timer Enable bits (Watch dog timer is always disabled. SWDTEN has no effect.) #pragma config WDTPS = 32768 // Watchdog Timer Postscale Select bits (1:32768)
// CONFIG3H #pragma config CCP2MX = PORTC1 // CCP2 MUX bit (CCP2 input/output is multiplexed with RC1) #pragma config PBADEN = ON // PORTB A/D Enable bit (PORTB<5:0> pins are configured as analog input channels on Reset) #pragma config CCP3MX = PORTB5 // P3A/CCP3 Mux bit (P3A/CCP3 input/output is multiplexed with RB5) #pragma config HFOFST = OFF // HFINTOSC Fast Start-up (HFINTOSC output and ready status are delayed by the oscillator stable status) #pragma config T3CMX = PORTC0 // Timer3 Clock input mux bit (T3CKI is on RC0) #pragma config P2BMX = PORTC0 // ECCP2 B output mux bit (P2B is on RB5) #pragma config MCLRE = INTMCLR // MCLR Pin Enable bit (RE3 input pin enabled; MCLR disabled)
// CONFIG4L #pragma config STVREN = ON // Stack Full/Underflow Reset Enable bit (Stack full/underflow will cause Reset) #pragma config LVP = ON // Single-Supply ICSP Enable bit (Single-Supply ICSP enabled if MCLRE is also 1) #pragma config XINST = OFF // Extended Instruction Set Enable bit (Instruction set extension and Indexed Addressing mode disabled (Legacy mode))
// CONFIG5L #pragma config CP0 = OFF // Code Protection Block 0 (Block 0 (000200-000FFFh) not code-protected) #pragma config CP1 = OFF // Code Protection Block 1 (Block 1 (001000-001FFFh) not code-protected)
// CONFIG5H #pragma config CPB = OFF // Boot Block Code Protection bit (Boot block (000000-0001FFh) not code-protected) #pragma config CPD = OFF // Data EEPROM Code Protection bit (Data EEPROM not code-protected)
// CONFIG6L #pragma config WRT0 = OFF // Write Protection Block 0 (Block 0 (000200-000FFFh) not write-protected) #pragma config WRT1 = OFF // Write Protection Block 1 (Block 1 (001000-001FFFh) not write-protected)
// CONFIG6H #pragma config WRTC = OFF // Configuration Register Write Protection bit (Configuration registers (300000-3000FFh) not write-protected) #pragma config WRTB = OFF // Boot Block Write Protection bit (Boot Block (000000-0001FFh) not write-protected) #pragma config WRTD = OFF // Data EEPROM Write Protection bit (Data EEPROM not write-protected)
// CONFIG7L #pragma config EBTR0 = OFF // Table Read Protection Block 0 (Block 0 (000200-000FFFh) not protected from table reads executed in other blocks) #pragma config EBTR1 = OFF // Table Read Protection Block 1 (Block 1 (001000-001FFFh) not protected from table reads executed in other blocks)
// CONFIG7H #pragma config EBTRB = OFF // Boot Block Table Read Protection bit (Boot Block (000000-0001FFh) not protected from table reads executed in other blocks)
// #pragma config statements should precede project file includes.
// Use project enums instead of #define for ON and OFF.
#include <xc.h> #include <stdio.h> #include <stdint.h> #include <string.h>
#define _XTAL_FREQ 64000000 #define DATA_SIZE (5) #define I2C_ACK 0x00 #define I2C_NACK 0xff #define ADRESS_PCA9685 0x80 // PCA9685 のデバイスアドレス
//#define SERVO_PULSE_MIN (500) #define SERVO_PULSE_MIN (1300)
//#define SERVO_PULSE_MAX (2400) #define SERVO_PULSE_MAX (1600)
int ReadCount;
int ReadFlag;
signed char ReadData[DATA_SIZE];
signed char SetData[DATA_SIZE];
signed char MoterSpeed;
int pulse_ratio = (SERVO_PULSE_MAX - SERVO_PULSE_MIN)/256;
void __interrupt () Interrupt();
void PICSetup();
void CCPSetup();
void InterruptSetup();
void SerialSetup();
void TimerSetup();
void I2C_setup();
void SerialWrite1(char,char,char);
void i2c_1_start();
void i2c_1_stop();
void i2c_1_write(uint8_t);
uint8_t i2c_1_CheckAck();
void write_PCA16(uint8_t,uint8_t,uint16_t);
void write_PCA8(uint8_t,uint8_t,uint8_t);
void init_servo();
void pos_servo(int,unsigned long);
void Moter();
int main(){
PICSetup();
CCPSetup();
InterruptSetup();
SerialSetup();
TimerSetup();
I2C_setup();
init_servo();
while(1){
MoterSpeed = SetData[0];
Moter();
pos_servo( 0,(unsigned long)((((int)SetData[1]*pulse_ratio)*(-1)) + ((SERVO_PULSE_MIN + SERVO_PULSE_MAX)/2)));
// __delay_ms(100);
}
}
void PICSetup(){
OSCCON = 0b01110000; //16MHz????????
OSCTUNEbits.PLLEN = 1; //???????PLL?4?? 64MHz
ANSELA = 0b00000000;
ANSELB = 0b00000000;
ANSELC = 0b00000000;
TRISA = 0b00000000;
TRISB = 0b00000000;
TRISC = 0b10011000;
LATA = 0b00000000;
LATB = 0b00000000;
LATC = 0b00000000;
}
void CCPSetup(){
CCPTMRS0 = 0b00000000; //CCP2 -> timer2
CCP2CON = 0b00001100; //PWM MODE
T2CON = 0b00000010; //TMR2?????? 1/16
TMR2 = 0; //TMR2COUNTER ???
CCPR2L = 0; //??????? ???
CCPR2H = 0;
PR2 = 249; //4000hz
TMR2ON = 1; //TMR2 START
}
void InterruptSetup(){
T0IF = 0; //タイマー0フラグ初期化
RC1IF = 0; //uart1フラグ初期化
SSP1IF = 0; //SSPフラグ初期化
BCL1IF = 0; //MSSPバス衝突フラグ初期化
GIE = 1; //割り込み許可
PEIE = 1; //周辺機器の割り込み許可
RC1IE = 1; //受信割り込み許可
T0IE = 1; //タイマー0割り込み許可
// SSP1IE = 1; //SSP割り込み許可
// BCL1IE = 1; //MSSPバス衝突割り込み許可
}
void SerialSetup(){
TXSTA1 = 0b00100100;
RCSTA1 = 0b10010000;
BAUDCON1 = 0b00001000;
SPBRG1 = 0b10001010; //34 115200
SPBRGH1 = 0b00000000;
CREN1 = 1;
}
void TimerSetup(){
T0CON = 0b10000111; //タイマー0有効 16μsで1カウント
TMR0 = 53036; //タイマー0レジスタの初期化 200msで割り込み
}
void I2C_setup(){
SSP1STAT = 0b10000000;
SSP1CON1 = 0b00101000;
SSP1CON3 = 0b00000000;
SSP1ADD = 159;
}
void __interrupt () Interrupt(){
GIE = 0;
if(T0IF){
TMR0 = 53036;
T0IF = 0;
}
if( FERR1 || OERR1 ){
CREN1 = 0;
NOP();
CREN1 = 1;
}
if(RC1IF){
ReadData[ReadCount] = RCREG1;
if(ReadData[ReadCount] == 127){
if(ReadCount == 4){
memcpy(SetData,ReadData,sizeof(ReadData));
ReadFlag = 1;
}
ReadCount = 0;
}else if(ReadCount >= 4){
ReadCount = 0;
}else{
ReadCount++;
}
RC1IF = 0;
}
GIE = 1;
}
void SerialWrite1(char data0,char data1,char data2){
char WriteData1[3];
WriteData1[0] = data0;
WriteData1[1] = data1;
WriteData1[2] = data2;
int i;
for (i = 0;i <= 2;i++){ //data0からdata4まで順番に送信
while(TX1IF == 0); //送信可能になるまで待つ
TXREG1 = WriteData1[i]; //iバイト目を送信
__delay_ms(5);
}
}
void i2c_1_start() {
// SSP1CON2レジスタのSENビットを1に設定すると
// スタートコンディションが生成される
// 発行が完了するとSSP1IFが1になるのでwhile文で待つ
SSP1IF = 0;
SSP1CON2bits.SEN = 1;
while (SSP1IF == 0) {}
SSP1IF = 0;
return;
}
void i2c_1_stop() {
// SSP1CON2レジスタのPENビットを1に設定すると
// ストップコンディションが生成される
// 発行が完了するとSSP1IFが1になるのでwhile文で待つ
SSP1IF = 0;
SSP1CON2bits.PEN = 1;
while (SSP1IF == 0) {}
SSP1IF = 0;
return;
}
void i2c_1_write(uint8_t data) {
// SSP1BUFに送信したいデータをセットすると、そのデータが送信される
// 発行が完了するとSSP1IFが1になるのでwhile文で待つ
SSP1IF = 0;
SSP1BUF = data;
while (SSP1IF == 0) {}
SSP1IF = 0;
return;
}
uint8_t i2c_1_CheckAck() {
uint8_t ackStatus;
if (SSP1CON2bits.ACKSTAT) {
ackStatus = I2C_NACK;
} else {
ackStatus = I2C_ACK;
}
return ackStatus;
}
void write_PCA16(uint8_t lowad,uint8_t reg,uint16_t data){
i2c_1_start();
i2c_1_write( ADRESS_PCA9685 + ( lowad << 1 ) ); // I2Cアドレス
i2c_1_write( reg ); // レジスタNo.
i2c_1_write( data & 0x00FF ); // 下位データ
i2c_1_write( data >> 8 ); // 上位データ
i2c_1_stop();
}
// I2Cデバイスへの8bitデータの書き込み
void write_PCA8(uint8_t lowad,uint8_t reg,uint8_t data ){
i2c_1_start();
i2c_1_write( ADRESS_PCA9685 + ( lowad << 1 ) ); // I2Cアドレス
i2c_1_write( reg ); // レジスタNo.
i2c_1_write( data ); // データ
i2c_1_stop();
}
void init_servo(){
int i,j;
// いったんスタンバイにする（プリスケーラ設定のため）
write_PCA8( 0, 0, 0x31 );
// プリスケーラ設定。分解能4us、周期20ms（周波数50Hz）！！デバイスごとに調整！！
write_PCA8( 0, 0xFE, 121 );
// MODE1設定 レジスタインクリメント、All-Call有効、アクティブ
write_PCA8( 0, 0, 0x21 );
// MODE2設定 トーテムポール出力設定
write_PCA8( 0, 1, 0x04 );
// パルス初期値書き込み(1500us)
j = 0x06;
for ( i = 0 ; i < 16 ; i++ ) {
write_PCA16( 0, j, 0 ); // パルススタート
j += 2;
write_PCA16( 0, j, 375 ); // パルスエンド
j += 2;
}
}
// サーボセット(サーボ位置をパルス幅で渡す)
void pos_servo(int id,unsigned long pos){
int i;
i = 8 + id * 4 ;
write_PCA16( 0, i, pos / 4 );
}
void Moter(){
if(MoterSpeed > 0){
PSTR2CON = 0b00000001;
CCPR2L = MoterSpeed*2;
}
if(MoterSpeed == 0){
PSTR2CON = 0b00000000;
CCPR2L = 0;
}
if(MoterSpeed < 0){
PSTR2CON = 0b00000010;
CCPR2L = MoterSpeed*(-2);
}
}この記事が気に入ったらサポートをしてみませんか？