UMEHOSHI ITA TOP PAGE COMPUTER SHIEN LAB
このページで示した作品の
[UMEHOSHI ITA]側プログラムです。
なおロボットの電源投入後に、約２分後にブザーがなります。これが鳴ればプログラムが[UMEHOSHI ITA]へ転送されて実行済み状態です。
ロボットのWifiアクセスポイント（SSID：pizero、passphrase：abcd1234）接続した後、
「umehoshiアプリ」などでTCPサーバ(192.168.100.1)に繋げてコマンドファイルを送信できれば、応答の動作ができるはずです。
なおこのセットアップと同じであれば、SSHサーバも起動しているはずですので、(pi@192.168.100.1 password:abc123)で接続して制御できるはずです。
#include <xc.h> // app_pwm_pizero.c
#include "common.h"
#define AdrStart 0x80005000 // アプリの起動時の初期ルーチンのアドレス
#define AdrTimer2 0x80005B00 // 指定時間後にモータ停止のTimer2用アドレス
#define AdrForward 0x80005200 // モータ前進ルーチンのアドレス
#define AdrGoBack 0x80005300 // モータ後進ルーチンのアドレス
#define AdrRight 0x80005400 // モータ右回転ルーチンのアドレス
#define AdrLeft 0x80005500 // モータ左回転ルーチンのアドレス
#define AdrUpLeft 0x80005600 // モータ左デューティ幅アップ
#define AdrDownLeft 0x80005700 // モータ左デューティ幅ダウン
#define AdrUpRight 0x80005800 // モータ右デューティ幅アップ
#define AdrDownRight 0x80005900 // モータ右デューティ幅ダウン
#define AdrStop 0x80005A00 // モータ停止
#define AdrBeep 0x80005D00 // ビープ音
#define UPDAOWN 0x1000 // デューティ幅の変更で使う値（この値を加算又は減算）
// 左右のモータで、進行にバラツキがある場合、下記初期設定を調整して合わせる。
int cn6PWM = 0x04FFF; // 現在の左デューティ幅で、初期設定
int cn7PWM = 0x04FFF; // 現在の右デューティ幅で、初期設定
int time2Count = 0;//Timer2割り込みでカウントアップ(下記stopCountまでカウント)
int stopCount = 0x500;// time2CountがstopCount以上で、time2CountがをゼロにしてモータをOFF
// 下記は、モータを片方ずつ交互に使う時に0以外を指定する。(低圧高電流型モータで使う場合あり）
int oneMCount = 0;// oneCN6～oneCN7の間で、oneCN6が負でTimer2でカウントアップ
int oneCN6 = 0;//-50;// CN6のONタイミングを負のTimer2でカウント数で指定
int oneCN7 = 50; // CN7のONタイミングをTimer2でカウント数で指定
void setCN6(int);// 左用 CN6コネクタの制御 dの値 0停止, 1:正転, -1:逆転
void setCN7(int);// 右用 CN7コネクタの制御 dの値 0停止, 1:正転, -1:逆転
void stop();// PWM を停止
void beep (void);// ビープ
__attribute__((address( AdrStart ))) void start (void);
void start()
{
PORTBCLR = 0b0110000000001100;//モータ関連ポート（14,13,3,2）をゼロにする
IEC0CLR = 0x00000200;//T2IE Timer2 Enable（割込み不可許可）");
// 初期化ルーチン（リセット時の処理）を設定
( (uint32_t *)_HANDLES)[_IDX_INIT_SUB_FUNC] = (uint32_t ) AdrStart;
__asm__ ("NOP");
// Timer2の割り込み処理を置き換える
( (uint32_t *)_HANDLES)[_IDX_TIMER_2_FUNC] = (uint32_t ) AdrTimer2;
PR2=0x8FFF; // Timer2の周期設定
_set_pwd_mode(1); // PWM モードへ変更
T2CONbits.ON = 1; //Timer2の機能を有効
time2Count =0;
_RB15 = 0; // LED D1を消灯
_clear_beep_code();
_set_beep_code((uint8_t)0b00100011);//音パターン[・・－]の登録
_UM_PTR_GOTO_BEEP = NULL;//ループ生成OFF
}
// タイマーで（stopCount 値に達するまで）の時間ごとに、モータを停止する。
// （モータがオンで、移動し続けないようにする考慮で、無くてもよい処理？）
__attribute__((address( AdrTimer2))) void timer2(void);
void timer2()
{
IFS0CLR = 0x00000200; // Clear the timer interrupt status flag
if( ++time2Count >= stopCount ){
stop();// PWM パルス幅を0
_RB15 = 0; // LED1消灯
}
// CN6,CN7のモータを交互に使う
if(oneCN6 < 0 && oneMCount++ < 0){
OC5CONbits.ON=OC1CONbits.ON=1; // PWM CN6有効
OC3CONbits.ON=OC4CONbits.ON=0; // PWM CN7無効
} else if(oneCN6 < 0 && oneCN7 > 0){
OC5CONbits.ON=OC1CONbits.ON=0; // PWM CN6無効
OC3CONbits.ON=OC4CONbits.ON=1; // PWM CN7有効
if( oneMCount > oneCN7){
oneMCount = oneCN6;
}
}
}
// モータを停止する。
__attribute__((address( AdrStop))) void stop(void);
void stop()
{
IEC0CLR = 0x00000200;//T2IE Timer2 Enable（割込み不可許可）");
OC5RS = OC1RS = OC4RS = OC3RS = 0;// モータすべてをOFF
time2Count = 0;
}
// モータ前進ルーチン
__attribute__((address( AdrForward ))) void forward(void);
void forward()
{
time2Count = 0;
stopCount = 350;
setCN6( 1 );// 左正転
setCN7( 1 );// 右正転
_RB15 = ! _RB15;//確認用のLED1点灯反転
IEC0SET = 0x00000200;//T2IE Timer2 Enable（割込み許可）");
}
// モータ後進ルーチン
__attribute__((address( AdrGoBack ))) void goback(void);
void goback()
{
time2Count = 0;
stopCount = 200;
setCN6( -1 );// 左逆転
setCN7( -1 );// 右逆転
_RB15 = ! _RB15;//確認用のLED1点灯反転
IEC0SET = 0x00000200;//T2IE Timer2 Enable（割込み許可）");
}
// モータ右回転ルーチン
__attribute__((address( AdrRight ))) void right(void);
void right()
{
time2Count = 0;
stopCount = 150;
setCN6( 1 );// 左正転
setCN7( -1 );// 右逆転
_RB15 = ! _RB15;//確認用のLED1点灯反転
IEC0SET = 0x00000200;//T2IE Timer2 Enable（割込み許可）");
}
// モータ左回転ルーチン
__attribute__((address( AdrLeft ))) void left(void);
void left()
{
time2Count = 0;
stopCount = 150;
setCN6( -1 );// 左逆転
setCN7( 1 );// 右正転
_RB15 = ! _RB15;//確認用のLED1点灯反転
IEC0SET = 0x00000200;//T2IE Timer2 Enable（割込み許可）");
}
// モータ左デューティ幅アップルーチン
__attribute__((address( AdrUpLeft ))) void upleft(void);
void upleft()
{
//_send_hex_low( cn6PWM );
if( cn6PWM +UPDAOWN <= 0x0ffff){
cn6PWM += UPDAOWN ;
//_send_string(" Left UP\r\n");
} else {
beep();
//_send_string(" Left FULL\r\n");
}
_RB15 = ! _RB15;//確認用のLED1点灯反転
}
// モータ左デューティ幅ダウンルーチン
__attribute__((address( AdrDownLeft ))) void downleft(void);
void downleft()
{
//_send_hex_low( cn6PWM );
if( cn6PWM - UPDAOWN >= 0){
cn6PWM -= UPDAOWN ;
//_send_string(" Left DOWN\r\n");
} else {
beep();
//_send_string(" Left Minimum\r\n");
}
_RB15 = ! _RB15;//確認用のLED1点灯反転
}
//モータ右デューティ幅アップ
__attribute__((address( AdrUpRight ))) void upright(void);
void upright()
{
//_send_hex_low( cn7PWM );
if( cn7PWM +UPDAOWN <= 0x0ffff){
cn7PWM += UPDAOWN ;
//_send_string(" Right UP\r\n");
} else {
beep();
//_send_string(" Right FULL\r\n");
}
_RB15 = ! _RB15;//確認用のLED1点灯反転
}
// モータ右デューティ幅ダウン
__attribute__((address( AdrDownRight ))) void downright(void);
void downright()
{
//_send_hex_low( cn7PWM );
if( cn7PWM -UPDAOWN >= 0){
cn7PWM -= UPDAOWN ;
//_send_string(" Right DOWN\r\n");
} else {
beep();
//_send_string("Right Minimum\r\n");
}
_RB15 = ! _RB15;//確認用のLED1点灯反転
}
__attribute__((address( AdrBeep ))) void beep (void);
void beep()
{
_clear_beep_code();
_set_beep_code((uint8_t)0b111100100);//音パターン[－－－－]の登録
_UM_PTR_GOTO_BEEP = NULL;//ループ生成OFF
}
// CN6（左側） コネクタの制御 dの値 0停止, 1:正転, -1:逆転
void setCN6( int d){
OC5RS = 0x0; // CN6[1-2]制御（RB2）
OC1RS = 0x0; // CN6[3-4]制御（RB3）
if(d==1){
OC5RS = cn6PWM; // CN6[1-2]制御（RB2）
OC1RS = 0x0; // CN6[3-4]制御（RB3）
} else if(d==-1){
OC5RS = 0x0; // CN6[1-2]制御（RB2）
OC1RS = cn6PWM; // CN6[3-4]制御（RB3）
}
}
// CN7（右側） コネクタの制御 dの値 0停止, 1:正転, -1:逆転
void setCN7( int d){
OC4RS = 0x0; // CN7[3-4]制御（RB13）
OC3RS = 0x0; // CN7[1-2]制御（RB14）
if(d==1){
OC4RS = cn7PWM; // CN7[3-4]制御（RB13）
OC3RS = 0x0; // CN7[1-2]制御（RB14）
} else if(d==-1){
OC4RS = 0x0; // CN7[3-4]制御（RB13）
OC3RS = cn7PWM; // CN7[1-2]制御（RB14）
}
}
上記をビルドしてできたファイルapp_pwm_pizero.c.hexの内容に、実行命令"R00800050000061"を追加した内容で、
「/home/pi/umehoshi/datas/app_pwm_pizero.c.hex」のパスになるように配置します。
このファイルは、ロボット起動時に「/home/pi/umehoshi/umehoshi.py」によって、[UMEHOSI IAT]に転送、実行されます。
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
forward R0080005200005F
back R0080005300005E
left R0080005500005C
right R0080005400005D
UpLeft R0080005600005B
DownLeft R0080005700005A
UpRight R00800058000059
DownRight R00800059000058
Stop R0080005A000050
Beep R0080005D00004D