졸작의뢰는 ▶LINK 로 와주세요

댓글 환영입니다.

http://www.sonsivri.to/forum/index.php?topic=59978.0

MPU-6050 6-Axis Accelerometer/Gyroscope with 16F877A - CCS C Code

Related to the acceleration sensors, 

many questions unanswered in the forums and saw the unfinished work. 

Codes associated with acceleration sensors, 

I'll be posting the help of the codes used in the forums.

관련된 to the 가속도 센서들에,

많은 질문들 답변없는 in the 포럼에서 and 본 the 미환성 작업을.

코드들 관련된 with 가속도 센서들과,

이건 will be 코드팅될것이다. the help에 of the 코드들의 사용된 in the 포럼들에서.

Some mathematical formulas taken from their pdf files of the acceleration sensor.

Some numbers, because it does not allow the processor bits 

I wrote using the appropriate numbers. 

CCS C is very efficient in this regard. 

8-bit inefficiency is able to absorb ("math.h"). 

Proton and Picbasic Pro may be errors. 

I'm glad you publish corrections.

We may in the future use of these codes to balance the robot and share code.

Good work everyone.

몇몇 수학적 공식들 취득한 from 그것들의 pdf 파일들에서 of the 가속도 센서의.

몇몇 숫자들 because it doesn't not 허락하지않기때문에. the 프로세서 비트들을

난 썼다. 사용하며 the 적합한 숫자들을.

CCSC는 매우 효율적이다. in 이 관련에서.

8비트 비효율은 가능게한다. to 없애는게

프로톤 and 픽베이직 프로는 may be 에러가될것이다.

난 기쁘다. 올바른걸 publish하게되서

우린 may in 미래에 사용할것이다. of 이 크도들을 to 균형잡기위해 the 로봇을 and 공유하자 코드를.

모두 즐거운 작업

▼▼▼▼▼▼▼▼▼▼▼▼▼▼

#include <16f877A.h>

#FUSES NOWDT                    //No Watch Dog Timer

#FUSES NOBROWNOUT               //No brownout reset

#FUSES NOLVP              //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O

#FUSES HS                       //Dont change

#use    delay(clock=20MHz)  

#use I2C(master, sda=PIN_d0, scl=PIN_d1, slow)  

//You can change, according your board ports

#use rs232(baud=9600,xmit=PIN_C6,rcv=PIN_C7)    

//You can change, according your board ports

#include "EXLCD.C"       //You can change, according your board ports

//#include "EXLCD416.C"  //You can change, according your board ports

#include "MPU6050.C"

#include <math.h>

signed int8 A_data[6];

signed int8 temp_data[2];

signed int8 G_data[6];

signed int16 Xa=0,Ya=0,Za=0;

signed int16 temp=0;

signed int16 Xg=0,Yg=0,Zg=0;

void main()

{

delay_ms(2);

lcd_init();

mpu6050_init();    

printf(lcd_putc,"\f");

lcd_gotoxy(1,1);

printf(lcd_putc," MPU6050 Gyro   ");

lcd_gotoxy(1,2);

printf(lcd_putc," Accelerometer  ");

delay_ms(1000);

printf(lcd_putc,"\f");

   while(TRUE)

   {

       A_data[0]=mpu6050_read(0x3B); //Read X axis(LSB)

       A_data[1]=mpu6050_read(0x3C); //Read X axis(MSB)

       A_data[2]=mpu6050_read(0x3D); //Read Y axis(LSB)

       A_data[3]=mpu6050_read(0x3E); //Read Y axis(MSB)

       A_data[4]=mpu6050_read(0x3F); //Read Z axis(LSB)

       A_data[5]=mpu6050_read(0x40); //Read Z axis(MSB)

       temp_data[0]=mpu6050_read(0x41);

       temp_data[1]=mpu6050_read(0x42);

       G_data[0]=mpu6050_read(0x43); //Read X axis(LSB)

       G_data[1]=mpu6050_read(0x44); //Read X axis(MSB)

       G_data[2]=mpu6050_read(0x45); //Read Y axis(LSB)

       G_data[3]=mpu6050_read(0x46); //Read Y axis(MSB)

       G_data[4]=mpu6050_read(0x47); //Read Z axis(LSB)

       G_data[5]=mpu6050_read(0x48); //Read Z axis(MSB)

       Xa=make16(A_data[0],A_data[1]);

       Ya=make16(A_data[2],A_data[3]);

       Za=make16(A_data[4],A_data[5]);

       temp=make16(temp_data[0],temp_data[1]);

       temp=temp/340 + 36.53;

       Xg=make16(G_data[0],G_data[1]);

       Yg=make16(G_data[2],G_data[3]);

       Zg=make16(G_data[4],G_data[5]);

       float Heading = atan2((signed int16)Ya,(signed int16)Xa)* 180 / pi + 180;

       lcd_gotoxy(1,1);

       printf(lcd_putc,"X:%ld  ",Xa);

       lcd_gotoxy(10,1);

       printf(lcd_putc,"Y:%ld  ",Ya);

       lcd_gotoxy(1,2);

       printf(lcd_putc,"Z:%ld  ",Za);

       lcd_gotoxy(10,2);

       printf(lcd_putc,"H:%f  ",heading);

/* For  4x16 LCD

       lcd_gotoxy(1,3);

       printf(lcd_putc,"Xg:%ld  ",Xg);

       lcd_gotoxy(9,3);

       printf(lcd_putc,"Yg:%ld  ",Yg);

       lcd_gotoxy(1,4);

       printf(lcd_putc,"Zg:%ld  ",Zg);

       lcd_gotoxy(9,4);

       printf(lcd_putc,"T :%ld  ",temp);

*/

   printf("\n\r H: %f X: %ld  Y: %ld  Z: %ld  T: %ld  \n\r",Heading,Xg,Yg,Zg,temp);

   delay_ms(100);

   }

}

▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲

▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼

// MPU6050 required Registers

#define W_DATA            0xD0

#define R_DATA            0xD1

#define PWR_MGMT_1     0x6B

#define PWR_MGMT_2     0x6C

#define SMPRT_DIV        0x19

#define CONFIG_R       0x1A

#define GYRO_CONFIG    0x1B

#define ACCEL_CONFIG   0x1C

#define ACCEL_XOUT_H   0x3B

#define ACCEL_XOUT_L   0x3C

#define ACCEL_YOUT_H   0x3D

#define ACCEL_YOUT_L   0x3E

#define ACCEL_ZOUT_H   0x3F

#define ACCEL_ZOUT_L   0x40

#define TEMP_OUT_H     0x41

#define TEMP_OUT_L     0x42

#define GYRO_XOUT_H    0x43

#define GYRO_XOUT_L    0x44

#define GYRO_YOUT_H    0x45

#define GYRO_YOUT_L    0x46

#define GYRO_ZOUT_H    0x47

#define GYRO_ZOUT_L    0x48

void mpu6050_write(int add, int data)

{

         i2c_start();

         i2c_write(W_DATA);

         i2c_write(add);

         i2c_write(data);

         i2c_stop();

}

int16 mpu6050_read(int add)

{

         int retval;

         i2c_start();

         i2c_write(W_DATA);

         i2c_write(add);

         i2c_start();

         i2c_write(R_DATA);

         retval=i2c_read(0);

         i2c_stop();

         return retval;

}

void mpu6050_init()

{

         mpu6050_write(PWR_MGMT_1,  0x80);

         delay_ms(100);

         mpu6050_write(PWR_MGMT_1,  0x00);

         delay_ms(100);

         mpu6050_write(CONFIG_R,    0x01);

         delay_ms(10);

         mpu6050_write(GYRO_CONFIG, 0x00);

}

▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲

http://playground.arduino.cc/Main/MPU-6050

The InvenSense MPU-6050 sensor contains 

a MEMS accelerometer and a MEMS gyro in a single chip.

이 MPU-6050 센서는 포함한다.

a MEMs 가속도 센서 and MEMS 자이로 in a 싱글 칩에.

It is very accurate, as it contains 

16-bits analog to digital conversion hardware 

for each channel.

이건 매우 정확하다, as 그게 포함하면서 

16비트 아날로그 to 디지털 변환 하드웨어 

for 각 채널을 위해.

Therefor it captures the x, y, and z channel 

at the same time.

따라서 이건 캡쳐한다. the x,y, and z 채널

at the 동시에.

The sensor uses the I2C-bus to interface 

with the Arduino.

The MPU-6050 is not expensive, 

especially given the fact that it combines 

both an accelerometer and a gyro.

센서는 사용한다. the I2C버스 to 인터페이스하기위해 

with the 아두이노.

The MPU-6050는 비싸지않다,

특히 주어져있다. the face가 that 그게 조합하는 

both an 가속도 and a 자이로 둘다.

Also note that Invensense has combined the MPU-6050

with a magnetometer (compass) 

in a single chip called MPU-9150.

또한 note that Invensense는 이미 조합했다. the MPU-6050

with a 자력계 (컴퍼스)

in a 싱글칩 불리는 MP-9150으로.

.

.

.

Short example sketch

The short example sketch is a very short sketch 

and it shows all the raw values 

(accelerometer, gyro and temperature). 

It should work on Arduino Uno, Nano, Leonardo, and also Due.

짧은 예제 sketch는 매우 짧다.

and 이건 보여준다. all the raw 값들을

(가속도센서, 자이로 and 온도).

이건 should 작업해야한다. on Arduino Uno, Nano, Leonardo, and also Due에서.

▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼

// MPU-6050 Short Example Sketch

// By Arduino User JohnChi

// August 17, 2014

// Public Domain

#include<Wire.h>

const int MPU_addr=0x68;  // I2C address of the MPU-6050

int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;

void setup()

{

      Wire.begin();

      Wire.beginTransmission(MPU_addr);

      Wire.write(0x6B);  // PWR_MGMT_1 register

      Wire.write(0);     // set to zero (wakes up the MPU-6050)

      Wire.endTransmission(true);

      Serial.begin(9600);

}

void loop()

{

    Wire.beginTransmission(MPU_addr);

    Wire.write(0x3B);  // starting with register 0x3B     (ACCEL_XOUT_H)

    Wire.endTransmission(false);

    Wire.requestFrom(MPU_addr,14,true);  

// request a total of 14 registers

     AcX=Wire.read()<<8|Wire.read();  

// 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)   

     AcY=Wire.read()<<8|Wire.read();  

// 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)

     AcZ=Wire.read()<<8|Wire.read();  

// 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)

     Tmp=Wire.read()<<8|Wire.read();  

// 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)

      GyX=Wire.read()<<8|Wire.read();  

// 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)

      GyY=Wire.read()<<8|Wire.read();  

// 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)

      GyZ=Wire.read()<<8|Wire.read();  

// 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

      Serial.print("AcX = "); Serial.print(AcX);

      Serial.print(" | AcY = "); Serial.print(AcY);

      Serial.print(" | AcZ = "); Serial.print(AcZ);

      Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53);  

//equation for temperature in degrees C from datasheet

      Serial.print(" | GyX = "); Serial.print(GyX);

      Serial.print(" | GyY = "); Serial.print(GyY);

      Serial.print(" | GyZ = "); Serial.println(GyZ);

      delay(333);

}

▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲▲

칼만필터.pdf

칼만필더란 ::: 평균치를 어림짐작해서 값을 산출하는것이다. 이때 위 아래로 튀는 값을 Noise로 간주한다. 크진않지만 평균치에 소폭 영향을 미치기도 한다.

#include<math.h>

#include<Wire.h>

struct GyroKalman

{

float x_angle, x_bias;

float p_00, p_01, p_10, p_11;

float Q_angle, Q_gyro;

float R_angle;

};

struct GyroKalman accX;

float accelAngleX = 0;

float accelAngleY = 0;

float accelAngleZ = 0;

byte buf[6];

int cnt = 0;

unsigned long lastread = 0;

static const float    R_angle = 0.5;    //.3 default

static const float     Q_angle = 0.01;    //0.01 (Kalman)

static const float     Q_gyro = 0.04;    //0.04 (Kalman)

const int lowW = -215;

const int highX = 221;

const int lowY = -215;

const int highY = 221;

const int lowZ = -215;

const int highZ = 255;

지진감지센서 (EarthQuake Click)

2369065.pdf

▼출처

https://www.mikroe.com/earthquake-click

SM-24 Geophone Element

http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Accelerometers/SM-24%20Brochure.pdf

http://www.meoworkshop.org/seismic-sundays-1-hello-world/

Ardu-base 지진감지센서 (=SM-24)

http://irnumall.co.kr/product/ardu-base-%EC%95%84%EB%91%90%EB%B2%A0%EC%9D%B4%EC%8A%A4-%EC%A7%80%EC%A7%84%EA%B0%90%EC%A7%80-%EC%84%BC%EC%84%9C/45/#none

https://blog.naver.com/gbtec

블로그내 검색으로 "지진감지" 타이핑

http://www.instructables.com/id/Arduino-Frequency-Detection/

주파수 감지

보통은 Freqency를 내보내지만 여기선 입력으로 들어갔을때의 경우를 다룬다.

http://www.robotstorehk.com/micromouse/micromouse.html

▲미로 구매 사이트

마이크로마우스주행알고리즘(세미나)-icefire7.hwp

micromouse.pdf

▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼

; Micromouse control and PWM Software

;

; Dirk Becker, 30/04/99, Race - version

; Sonja Lenz

; Michael Gims

;

; Group B2

;

; Port RA1, RA0 as input (Speed up/down)

; Port Rpwm2, RB4 as PWM output

;

;

; RA00 --> Sensor 3 (all Sensor inputs are active High)

; RA01 --> Sensor 4

; RA02 --> Start/test engine button (Active High)

; RA03 --> Sensor 5

; RA04 --> Pulse IR-diode out (via FET) (active Low output)

; RB00 --> Sensor 6

; RB01 --> Sensor 7

; RB02 --> PWM 1 - Brake Out (High= Brake ON)

; RB03 --> PWM 2 - Brake Out (High= Brake ON)

; RB04 --> PWM 1 - Output (High= Pulse ON) (Left )

; RB05 --> PWM 2 - Output (High= Pulse ON) (Right)

; RB06 --> Sensor 1

; RB07 --> Sensor 2

;

;

;    Sensor Location (Bit locations)

;

; +------------------------------------+

; I                                     I

; I     L                     4          I

; I     M                     0         I

; I     1                     1           I

; I                           1          I

; I                                     I front

; I     L                     P         I ---------->

; I     M                     I         I

; I     2                     C         I

; I                                     I

; I                                     I

; +-----+-+-----------------------+-+--+

;       I   I                       I  I

;       I   I                        I  I

;       I   I                        S-2

;        S-1                        S-0

;       I   I                        S-3

;        +-+                         +-+

;

; The PWM and Brake outputs are hardware protected against

; not allowed conditions in order to prevent the

; transistors from damage

;

;

;

LIST P=16f84;f=inhx8m

ERRORLEVEL     -302     ; suppress bank selection messages

_CP_OFF         equ     H'3FFF'     ;code protect off

_PWRTE_ON     equ     H'3FFF'     ;Power on timer on

_WDT_OFF     equ     H'3FFB'     ;watch dog timer off

_HS_OSC     equ     H'3FFE'     ;crystal oscillator

__CONFIG     _CP_OFF & _PWRTE_ON & _WDT_OFF &_HS_OSC

; *************************************

; * DEFINITIONS *

; *************************************

pcl     equ 2 ; Registers

status  equ 3

porta     equ 5

portb     equ 6

intcon     equ 0bh

trisa     equ 85h

trisb     equ 86h

optreg     equ 81h

tmr0     equ 01h

c equ 0                 ; Bits in status

z equ 2

rp0 equ 5

out1 equ 4                 ; PWM Output Bits

out2 equ 5

brk1 equ 2             ; Brake Ouput Bits

brk2 equ 3

btn equ 2             ; Button (Port A) Pressed=1

diode equ 4         ; Pulse Out for IR-Diodes

toif equ 2         ; Bits in intcon

w equ 0         ; Register destinations

f equ 1         ;

stackw equ 0ch     ; stack to push pop the W-register ;

Variables

stacks equ 0dh     ; stack to push pop the Status-register

pwm1 equ 0eh     ; Ram address for PWM1 setting

pwm2 equ 0fh     ; Ram address for PWM2 setting

pwmhelp equ 010h     ; Ram address for PWM settings saving

n equ 011h     ; Ram address for variable n

m equ 012h     ; Ram address for variable m

k equ 013h     ; --- "" --- k

pulse equ 014f     ; Helping counter for pulses

sensors equ 015h     ; For IR-sensor

walls equ 016h     ; Count number of walls

l equ 017h     ; Ram address for variable l

pwmmax equ .230     ; PWM maximum speed

wait equ .2     ; Wait delay time

maxspd equ .7     ; Max speed the mouse can drive

wallcnt equ .20     ; Walls to drive around

goto init

; **********************************************************

; * ISR Routine *

; * needs 0..F in pwm1 and pwm2 for generating pwm pulses *

; * outputs directly to the motors *

; **********************************************************

org 04h

pwmisr

movwf stackw ; copy W-register to save stack

swapf status, w ; Swap Status-reg to be saved in W

movwf stacks ; Save Status to stacks

movf pwm1, w ; Compare pwmhelp-counter

subwf pwmhelp, w ; with PWM1 setting

btfsc status, c ; and set PWM1 Output to

bcf portb, out1 ; on or

btfss status, c

bsf portb, out1 ; off

movf pwm2, w ; Compare pwmhelp-counter

subwf pwmhelp, w ; with PWM1 setting

btfsc status, c ; and set PWM1 Output to

bcf portb, out2 ; on or

btfss status, c

bsf portb, out2 ; off

incf pwmhelp, f ; Increase PWM reference counter

bcf pwmhelp, 4 ; But not >0Fh

movlw pwmmax ; sets w register

bcf status, rp0 ; select bank 0

movwf tmr0 ; Set TMR0 to desired PWM resolution value

bcf intcon, toif ; Clear interrupt flag

swapf stacks, w ; Swap nibbles in stacks reg

; and place into W

movwf status ; restore Status-register

swapf stackw, f ; Swap nibbles in stackw and place into stackw

swapf stackw, w ; swap nibbles in stackw and restore to W-register

retfie

; ISR END

; *****************************************************

; * The initalisation is all done here *

; *****************************************************

init

bcf status, rp0 ; select bank 0

clrf porta ; set porta 0

bsf status, rp0 ; select bank 1

movlw 0fh ; set port A0 & A1

movwf trisa ; as input

bcf status, rp0 ; select bank 0

clrf portb ; set porta 0

bsf status, rp0 ; select bank 1

movlw 0c3h ; set port pwm1 & pwm2 & BRK1 & BRK2

movwf trisb ; as output

bsf status, rp0 ; select bank 1

movlw 40h ; select TMR0

movwf optreg ; as counter with no divider

bcf status, rp0 ; select bank 0

movlw 0e0h ; Enable TMR0

movwf intcon ; interrupts

clrf pwmhelp ; Clear PWMHELP address,

clrf pwm1 ; PWM1

clrf pwm2 ; and PWM2

clrf n ; clear variables

clrf m

clrf k

clrf sensors

bsf porta, diode ; Turn off IR-diodes

start

movlw 0 ; sets w register

bcf status, rp0 ; select bank 0

movwf tmr0 ; Set TMR0 to desired PWM resolution value

; **********************************************************

; * M A I N P R O G R A M M E *

; **********************************************************

main

bsf porta, diode ; Turn Off diodes (It's safer)

movlw wallcnt ; Set wallcounter

movwf walls

call btnpress

bcf portb, brk1

bcf portb, brk2

movlw 00h

movwf pwm1

movwf pwm2

; ****************************** The Mouse routines start here

***************

doagain

call straight ; Go ahead

call turn ; Drive the curve

call stop

call delay

decfsz walls, f

goto doagain

goto main ; End main function ------------------------------

; **********************************************************

; * Procedure Btnpress *

; * waits until Starting Button is pressed once and *

; * released *

; **********************************************************

;

btnpress

btfss porta, btn ; is start

goto btnpress ; button pressed?

nop

movlw 50 ; ca 50us

movwf k ; loop for

loop2

incfsz k, f ; preventing

goto loop2 ; button from jittering

loop1

btfsc porta, btn ; is the button

goto loop1 ; released again?

nop

return

; **********************************************************

; * Procedure Checksensors *

; * checks the IR-Diodes and writes the result into the *

; * so called sensor variable sensor - sensor1 is bit 0 *

; * it also provides all the timing needed for the sensors *

; **********************************************************

;

checksensors

clrf sensors ;

bcf porta, diode ; Turns the diodes ON

movlw .20 ; Set counter register

movwf k ; to delay

loop4

decfsz k, f ; for sensor

goto loop4 ; checking

bsf porta, diode ; Turn the Sensors off again

btfsc portb, 7 ; Check sensor 1

bsf sensors, 0 ;

btfsc portb, 6 ; Check sensor 2

bsf sensors, 1 ;

btfsc porta, 1 ; Check sensor 3

bsf sensors, 2 ;

btfsc porta, 0 ; Check sensor 4

bsf sensors, 3 ;

movf sensors, w

movlw 0ffh ; Delay to turn the sensors

movwf k ; long enough off

loop5

decfsz k, f ;

goto loop5 ;

return

; **********************************************************

; * Subroutine Stop *

; * Stops the engine WITH setting brakes *

; **********************************************************

;

stop

bsf portb, brk1 ;Stop both motors and brake them

bsf portb, brk2

clrw

movwf pwm1

movwf pwm2

return

; **********************************************************

; * Subroutine Delay *

; * provides a delay *

; **********************************************************

;

delay

movlw .255

movwf n

stopagain1

movlw .146

movwf k

stopagain2

movlw 1

movwf m

stopagain3

incfsz m, f

goto stopagain3

incfsz k, f

goto stopagain2

incfsz n, f

goto stopagain1

return

; **********************************************************

; * Subroutine straight *

; * Drives straight along the wall *

; **********************************************************

;

straight

movlw .10

movwf n

straighton

bcf portb, brk1 ; release the brakes

bcf portb, brk2

call checksensors

clrw

btfsc sensors, 0

call straightrun

clrw

btfsc sensors, 3 ; Has right sensor wall contact?

call slow2 ; --> Yes: Jump slow 2

clrw

btfsc sensors, 2 ; Has left sensor wall contact?

call slow1 ; ; --> Yes: Jump slow 2

bcf sensors, 1 ; Sensor 1 should not be in use

movf sensors, w

btfss status, z

goto straighton

decfsz n, f

goto straighton

call stop

call delay

btfsc sensors, 0 ; Was it a gap?

goto straight ; --> Do it again

btfsc sensors, 2 ; ---- "" -----

goto straight

btfsc sensors, 3 ; ---- "" -----

goto straight

call stop

call delay

return

; **********************************************************

; * Subroutine slow1 *

; * Drives PWM1, with checking sensor 2--> result in w *

; **********************************************************

;

slow1

movlw maxspd

movwf pwm2

movlw maxspd-2 ; If only outer sensor has wall contact steer left

btfsc sensors, 0

movlw maxspd-1 ; If mid and outer sensor have contact steer

slightly

movwf pwm1

return

; **********************************************************

; * Subroutine slow2 *

; * Drives PWM2, with checking sensor 3 --> result in w *

; **********************************************************

;

slow2

movlw maxspd

movwf pwm1

movlw maxspd-2 ; If only outer sensor has wall contact steer right

btfsc sensors, 0 ;

movlw maxspd-1 ; If mid and outer sensor have contact steer

slightly

movwf pwm2

return

; **********************************************************

; * Subroutine Turn *

; * This subrutine drives right around the corner *

; **********************************************************

;

turn

call checksensors ; Check 'em again

bsf portb, brk2 ; Stop the

clrw ; right wheel

movwf pwm2 ; (BRAKE IT!)

bcf portb, brk1 ; Turn with Left wheel

movlw .4

movwf pwm1

btfss sensors, 3

goto turn

return

END ; it's enough now