上記の広告は1ヶ月以上更新のないブログに表示されています。
新しい記事を書く事で広告が消せます。
--.--.-- -- l スポンサー広告 l top
新年あけましておめでとうございます。本年もよろしくお願いいたします。

さて、昨日から暇を見てTARSを改良してみました。


まず、回路改修。

サーボモータの電源を電池から直接撮っていたのですが、使っているサーボSG-90の電圧範囲は4.8~5Vなので、秋月の5V三端子DC-DC M78AR05を追加して、電池も7.5Vとしました。電圧範囲はつい手抜きをしていたのですが、どうもサーボが振動するし、電池の状態によって動きが変わるのでちゃんとすることにしました。
正直言いますともう一つ手抜きをしています。サーボSG-90の制御信号も本当は4.8~5V規定なのですが、PICマイコンの3.3Vで直接動かしています。こちらは手持ちの物で試したら動いたのでそのままにしていますが、物によっては制御できない場合もあり得ますので、同じものをお使いになる方は5V規定でお使いください。

次に塗装やり直し。元々塗りが甘かったところ、その後の加工でかなり傷んできたので、ハードが固まったところで塗り直しました。


TARSの顔、というかモニタ表示部に黒テープを貼って完成したのが、一番上の写真です。



メイキングと動作を動画にしました。

前にはまあまあ進むのですが、左右方向制御はうまくいってません。
動画の最後で一周していますが、申し訳ありませんが、これはたまたまです。
プログラム的には前進、右旋回、前進、左旋回の繰り返しのはずなんですが。。。。

プログラムは以下の通りです。
※#includeで”<”、”>”が全角になっていますので、半角にしてください。
なぜかFC2ブログでは半角だとその行が化けてしまいます。

/*
* File: main.c (Mini TARS)
*
* Created on 2015/1/2, 19:34
*/

#include <stdio.h>
#include <stdlib.h>
#include "xc.h"


// FBS

// FGS
#pragma config GWRP = OFF // General Segment Code Flash Write Protection bit (General segment may be written)
#pragma config GCP = OFF // General Segment Code Flash Code Protection bit (No protection)

// FOSCSEL
#pragma config FNOSC = FRC // Oscillator Select (8 MHz FRC oscillator with divide-by-N (FRCDIV))
#pragma config IESO = ON // Internal External Switch Over bit (Internal External Switchover mode enabled (Two-Speed Start-up enabled))

// FOSC
#pragma config POSCMOD = HS // Primary Oscillator Configuration bits (HS Oscillator mode selected)
#pragma config OSCIOFNC = ON // CLKO Enable Configuration bit (CLKO output disabled)
#pragma config POSCFREQ = HS // Primary Oscillator Frequency Range Configuration bits (Primary oscillator/external clock input frequency greater than 8 MHz)
#pragma config SOSCSEL = SOSCHP // SOSC Power Selection Configuration bits (Secondary oscillator configured for high-power operation)
#pragma config FCKSM = CSDCMD // Clock Switching and Monitor Selection (Clock switching is disabled, Fail-Safe Clock Monitor is disabled)

// FWDT
#pragma config WDTPS = PS32768 // Watchdog Timer Postscale Select bits (1:32,768)
#pragma config FWPSA = PR128 // WDT Prescaler (WDT prescaler ratio of 1:128)
#pragma config WINDIS = OFF // Windowed Watchdog Timer Disable bit (Standard WDT selected; windowed WDT disabled)
#pragma config FWDTEN = OFF // Watchdog Timer Enable bit (WDT disabled (control is placed on the SWDTEN bit))

// FPOR
#pragma config BOREN = BOR3 // Brown-out Reset Enable bits (Brown-out Reset enabled in hardware; SBOREN bit disabled)
#pragma config PWRTEN = OFF // Power-up Timer Enable bit (PWRT disabled)
#pragma config BORV = LPBOR // Brown-out Reset Voltage bits (Low-Power Brown-out reset occurs around 2.0V)
#pragma config MCLRE = ON // MCLR Pin Enable bit (MCLR pin enabled; RA5 input pin disabled)

// FICD
#pragma config ICS = PGx2 // Reserved (PGC2/PGD2 are used for programming the device)

// FDS
#pragma config DSWDTPS = DSWDTPSF // Deep Sleep Watchdog Timer Postscale Select bits (1:2,147,483,648 (25.7 Days))
#pragma config DSLPBOR = OFF // Deep Sleep Zero-Power BOR Enable bit (Deep Sleep BOR disabled in Deep Sleep)
#pragma config DSWDTEN = OFF // Deep Sleep Watchdog Timer Enable bit (DSWDT disabled)

#define OFST1 -10
#define OFST2 12
#define OFST3 -8

int deg2duty(int);
void setpwm(void);
void waitms(int);
void setmotors(int, int , int , int, int, int,int);


int period1,period2,period3;
int duty1,duty2,duty3;
int tim0 = 0;
/*
*
*/
int main(int argc, char** argv) {
int i,j,y;
OSCCON = 0x07700;
CLKDIV = 0x0000;
AD1PCFG = 0xFFFF;
TRISA = 0x0000;
TRISB = 0x0000;
LATA = 0x5555;
LATB = 0x5555;

PR1 = 200; // 50us
IPC0bits.T1IP = 5;
T1CON = 0x8000;
IFS0bits.T1IF = 0;
IEC0bits.T1IE = 1;

waitms(3000);


duty1 =0; duty2 = 0; duty3 =0;
setmotors(0,1,0,1,0,1,10);
waitms(500);

while(1) {
gostraight(3);
waitms(100);
turnright(2);
waitms(100);
zitabata(1);
waitms(100);
gostraight(3);
waitms(100);
turnleft(2);
waitms(100);
zitabata(1);
waitms(100);
}

return (EXIT_SUCCESS);
}

void zitabata(int t) {
int i;
setmotors(-10,1,0,1,-10,1,10);
waitms(50);
for (i=0;i setmotors(-10,1,-30,1,-40,1,20);
waitms(50);
setmotors(-10,1,0,1,-10,1,20);
waitms(50);
setmotors(-40,1,30,1,-10,1,20);
waitms(50);
setmotors(-10,1,0,1,-10,1,20);
waitms(50);
}
setmotors(-5,1,0,1,-5,1,10);
waitms(50);
}

void turnright(int t){
int i;
for (i=0;i setmotors(-80,1,0,1,-80,1,3);
waitms(50);
setmotors(-35,1,0,1,-80,1,3);
waitms(50);
setmotors(-15,1,0,1,-15,1,10);
waitms(50);
}
setmotors(-5,1,0,1,-5,1,10);
waitms(50);
}

void turnleft(int t){
int i;
for (i=0;i setmotors(-80,1,0,1,-80,1,3);
waitms(50);
setmotors(-80,1,0,1,-35,1,3);
waitms(50);
setmotors(-15,1,0,1,-15,1,10);
waitms(50);
}
setmotors(-5,1,0,1,-5,1,10);
waitms(50);
}


void gostraight(int t){
int i;
for (i=0;i setmotors(-80,1,0,1,-80,1,3);
waitms(50);
setmotors(-5,1,0,1,-5,1,10);
waitms(50);
}

}

void setmotors(int target1, int step1, int target2, int step2, int target3, int step3,int t0) {
int flag = 0;
int cur1,cur2,cur3;

cur1 = duty1;cur2=duty2;cur3=duty3;

while (flag != 7) {
//************ MOTOR 1 SETTING
if ((flag & 0x01)==0) {
if (cur1 == target1) { flag |=1;}
if (cur1 < target1) {
cur1 += step1;
if (cur1 >= target1) {cur1 = target1;flag |=1;}
}
if (cur1 > target1) {
cur1 -= step1;
if (cur1 <= target1) {cur1 = target1;flag |=1;}
}
}
//************ MOTOR 2 SETTING
if ((flag & 0x02)==0) {
if (cur2 == target2) { flag |=2;}
if (cur2 < target2) {
cur2 += step2;
if (cur2 >=target2) {cur2 = target2;flag |=2;}
}
if (cur2 > target2) {
cur2 -= step2;
if (cur2 <= target2) {cur2 = target2;flag |=2;}
}
}
//************ MOTOR 2 SETTING
if ((flag & 0x04)==0) {
if (cur3 == target3) { flag |=4;}
if (cur3 < target3) {
cur3 += step3;
if (cur3 >=target3) {cur3 = target3;flag |=4;}
}
if (cur3 > target3) {
cur3 -= step3;
if (cur3 <= target3) {cur3 = target3;flag |=4;}
}
}
duty1 = cur1;duty2=cur2;duty3 = cur3;
setpwm();waitms(t0);
}
}

void waitms(int t){
tim0 = 0;
int t0 =0;

for (t0=0;t0 tim0 = 0;
while (tim0<20){
}
}
}

void setpwm(void) {
period1 =deg2duty(duty1+OFST1); period2=deg2duty(duty2+OFST2);period3=deg2duty(-duty3+OFST3);
}

int deg2duty(int deg){
int duty;
duty = 10+ (deg+90)*38/180;
return duty;
}

void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void)
{
static int count = 0;

if (tim0<10000) {
tim0++;
}
count++;
if (count >= 400) {
count = 0;
LATBbits.LATB8 = 1;
LATBbits.LATB9 = 1;
LATAbits.LATA4= 1;
}
if (count>=period2) LATBbits.LATB8 = 0;
if (count>=period3) LATBbits.LATB9 = 0;
if (count>=period1) LATAbits.LATA4= 0;

IFS0bits.T1IF = 0;
}


歩かせるのにかなり苦労しました。
もう少し改良してからそのあたり解説したいと思います。

それではまたです。
2016.01.03 Sun l 未分類 l コメント (0) l top

コメント

コメントの投稿












上記広告は1ヶ月以上更新のないブログに表示されています。新しい記事を書くことで広告を消せます。