TJ3Bで「Adafruit モーター/ステッパー/サーボ シールドキット 」v2を使う
Arduinoライブラリを参考にして、C-Styleに手を加えることでTJ3Bから動かせるようになった。
- Adafruit モーター/ステッパー/サーボ シールドキット v2 - スイッチサイエンス
- GitHub - adafruit/Adafruit_Motor_Shield_V2_Library: v2!
1モーターを制御するためにI2Cで通常5バイト×2回の通信が必要なため、6CHモーターコントローラの6モーター制御に6バイト×1回と比較し、効率が悪い(3モーターで5バイト×6回)ので、最大定格が高くても(1Chあたり、2A --> 3.2A)メリットはなさそう。
TJ3B用C-Style Ver.141024を使った時の手順は以下のとおり。
なお、改造は自己責任でお願いします。
(1)「C:\Daisen\Build\」フォルダの「Build_V141022」フォルダをリネームしてコピー(例 Build_V141022_Adafruit)。
(2)コピー先フォルダのcl2620.batの修正
setlocal set Mcu=18f2620 set Dst=\Daisen\mcc18_v337 set Src0=c018i set Src1=D_Main set Src2=D_I2c set Src3=D_Sio set SrcU=User set SrcA=Adafruit set OBJ0=%Src0%.o set OBJ1=%Src1%.o set OBJ2=%Src2%.o set OBJ3=%Src3%.o set OBJU=%SrcU%.o set OBJA=%SrcA%.o mcc18 -p %Mcu% -I %Dst%\h;. %Src0%.c mcc18 -p %Mcu% -I %Dst%\h;. %Src1%.c mcc18 -p %Mcu% -I %Dst%\h;. %Src2%.c mcc18 -p %Mcu% -I %Dst%\h;. %Src3%.c mcc18 -p %Mcu% -I %Dst%\h;. %SrcU%.c mcc18 -p %Mcu% -I %Dst%\h;. %SrcA%.c mplink %Mcu%.lkr %OBJ0% %OBJ1% %OBJ2% %OBJ3% %OBJU% %OBJA% /o %SrcU%.out /l %Dst%\lib /p %Mcu% endlocal
(3)同フォルダにファイル追加
Adafruit.h
/** * Adafruit モーター/ステッパー/サーボ シールドキット v2対応 * 2015-03 ohguma */ #ifndef USE_ADAFRUIT_MOTOR_DRIVER #define USE_ADAFRUIT_MOTOR_DRIVER 1 #define FORWARD 1 #define BACKWARD 2 #define BRAKE 3 #define RELEASE 4 extern void Adafruit_begin(); extern void Adafruit_speed(long sp1, long sp2, long sp3); long map(long x, long in_min, long in_max, long out_min, long out_max); void Adafruit_speedsub(int no, long sp); extern void Adafruit_setPWM(BYTE pin, UINT value); extern void Adafruit_setPin(BYTE pin, BOOL value); extern void Adafruit_run(BYTE num, BYTE cmd); extern void Adafruit_setSpeed(BYTE num, BYTE speed); #define _i2caddr 0xC0 #define PCA9685_MODE1 0x0 #define PCA9685_PRESCALE 0xFE #define LED0_ON_L 0x6 #define MT1_PWM 8 #define MT1_IN1 10 #define MT1_IN2 9 #define MT2_PWM 13 #define MT2_IN1 11 #define MT2_IN2 12 #define MT3_PWM 2 #define MT3_IN1 4 #define MT3_IN2 3 #define MT4_PWM 7 #define MT4_IN1 5 #define MT4_IN2 6 extern void Adafruit_pwm_servo_setPWM(BYTE num, UINT von, UINT voff); extern BYTE Adafruit_pwm_servo_read8(BYTE addr); extern void Adafruit_pwm_servo_write8(BYTE addr, BYTE d); #endif
Adafruit.c
/** * Adafruit モーター/ステッパー/サーボ シールドキット v2対応 * 2015-03 ohguma */ #include "D_MAIN.h" #include "D_I2C.h" #include "Adafruit.h" // sp = 100:100%CW, -100:100%CCW, 128:BRAKE void Adafruit_speed(long sp1, long sp2, long sp3) { Adafruit_speedsub(0, sp1); Adafruit_speedsub(1, sp2); Adafruit_speedsub(2, sp3); } void Adafruit_speedsub(int no, long sp) { if (sp == 128 || sp == -128) { Adafruit_run(no, BRAKE); Adafruit_setSpeed(no, 255); } else if (0 < sp && sp <= 100) { Adafruit_run(no, FORWARD); Adafruit_setSpeed(no, map(sp, 0, 100, 0, 255)); } else if (-100 <= sp && sp < 0) { sp = -sp; Adafruit_run(no, BACKWARD); Adafruit_setSpeed(no, map(sp, 0, 100, 0, 255)); } else { Adafruit_run(no, RELEASE); } } long map(long x, long in_min, long in_max, long out_min, long out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } //--------------------------------------------------------------------- void Adafruit_begin(void) { BYTE i; BYTE prescale; BYTE oldmode; BYTE newmode; Adafruit_pwm_servo_write8(PCA9685_MODE1, 0x0); /* freq = 1000の時 25,000,000 / 4,096 / 1,000 - 1 = 5.1035.... 5.1 + 0.5 の切り捨て */ prescale = 5; oldmode = Adafruit_pwm_servo_read8(PCA9685_MODE1); newmode = (oldmode & 0x7F) | 0x10; // sleep Adafruit_pwm_servo_write8(PCA9685_MODE1, newmode); // go to sleep Adafruit_pwm_servo_write8(PCA9685_PRESCALE, prescale); // set the prescaler Adafruit_pwm_servo_write8(PCA9685_MODE1, oldmode); wait_ms(5); Adafruit_pwm_servo_write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment. // This is why the beginTransmission below was not working. for (i=0; i<16; i++) { Adafruit_pwm_servo_setPWM(i, 0, 0); } } void Adafruit_setPWM(BYTE pin, UINT value) { if (value > 4095) { Adafruit_pwm_servo_setPWM(pin, 4096, 0); } else { Adafruit_pwm_servo_setPWM(pin, 0, value); } } void Adafruit_setPin(BYTE pin, BOOL value) { if (value == LOW) { Adafruit_pwm_servo_setPWM(pin, 0, 0); } else { Adafruit_pwm_servo_setPWM(pin, 4096, 0); } } void Adafruit_run(BYTE num, BYTE cmd) { BYTE in1; BYTE in2; if (num == 0) { in1 = MT1_IN1; in2 = MT1_IN2; } else if (num == 1) { in1 = MT2_IN1; in2 = MT2_IN2; } else if (num == 2) { in1 = MT3_IN1; in2 = MT3_IN2; } else if (num == 3) { in1 = MT4_IN1; in2 = MT4_IN2; } switch (cmd) { case FORWARD: Adafruit_setPin(in2, LOW); // take low first to avoid 'break' Adafruit_setPin(in1, HIGH); break; case BACKWARD: Adafruit_setPin(in1, LOW); // take low first to avoid 'break' Adafruit_setPin(in2, HIGH); break; case BRAKE: Adafruit_setPin(in1, HIGH); Adafruit_setPin(in2, HIGH); break; case RELEASE: Adafruit_setPin(in1, LOW); Adafruit_setPin(in2, LOW); break; } } void Adafruit_setSpeed(BYTE num, BYTE speed) { BYTE pwm; if (num == 0) { pwm = MT1_PWM; } else if (num == 1) { pwm = MT2_PWM; } else if (num == 2) { pwm = MT3_PWM; } else if (num == 3) { pwm = MT4_PWM; } Adafruit_setPWM(pwm, (UINT)speed * 16); } void Adafruit_pwm_servo_setPWM(BYTE num, UINT von, UINT voff) { BYTE n = 0; gI2C_Buf[n++] = (LED0_ON_L + 4 * num); gI2C_Buf[n++] = (BYTE) (von & 0xff); gI2C_Buf[n++] = (BYTE) ((von >> 8) & 0xff); gI2C_Buf[n++] = (BYTE) (voff & 0xff); gI2C_Buf[n++] = (BYTE) ((voff >> 8) & 0xff); i2c_send(_i2caddr, n); } BYTE Adafruit_pwm_servo_read8(BYTE addr) { BYTE status; gI2C_Buf[0] = addr; i2c_send(_i2caddr, 1); i2c_recv(_i2caddr, 1); status = gI2C_Buf[0]; return(status); } void Adafruit_pwm_servo_write8(BYTE addr, BYTE d) { gI2C_Buf[0] = addr; gI2C_Buf[1] = d; i2c_send(_i2caddr, 2); }
(4)Main.cのmain()に追記
: #ifdef USE_DSR1303 set_dsr1303(I2C_WCMD_STOP); #endif Adafruit_begin(); //追記 Adafruit_speed(0,0,0); //追記 gW_Tct = 0; while (LOOP){ if (rx_chk()) rx_job(); :
(5)C-Styleのプログラム中では6CHモータ制御ボタンの代わりにC-Codeを使いモーターを制御する。
- 「Adafruit_speed(0,0,0);」
- 「Adafruit_speed(gV[VAR_L], gV[VAR_M], gV[VAR_N]);」
(6)ビルド前にメニュー「プロジェクト」のビルドパスを選択し、(1)でコピーしたフォルダにある「BuildPath.BAT」を開く。
(7)ビルドする。
6CHモーターコントローラを使うは、ビルドパスを元に戻す。