Hatena::ブログ(Diary)

熊工房 ohgumaの腹凹ませたい日記

2004 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2005 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2006 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2007 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2008 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2009 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2010 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2011 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2012 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2013 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2014 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2015 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2016 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2017 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |
2018 | 01 | 02 | 03 | 04 | 05 | 06 | 07 | 08 | 09 | 10 | 11 | 12 |

2015-03-29

[]TJ3Bで「Adafruit モーター/ステッパー/サーボ シールドキット 」v2を使う

Arduinoライブラリを参考にして、C-Styleに手を加えることでTJ3Bから動かせるようになった。

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モーターコントローラを使うは、ビルドパスを元に戻す。

2015-03-18

[]TJ3B オムニのハード完成

f:id:ohguma:20150318213241j:image:w360

TJ3Bを使った3輪オムニロボのハードがやっと完成した。

キッカーの調整をしていたら、なにやら香ばしい匂いと煙が。

急ぎ電源を切り確認するも、目視では問題箇所が分からない。

改めて電源を入れると、きちんと動いているように見えるが、CN9のPINGポートが300cmに張り付いたまま。

超音波センサは別ポートにつなげば動くため、マイコン側の問題かも。