1 | #include <18F27J53.h>
|
2 | #device ADC=12 // nicht ver?ndern
|
3 | #fuses HSPLL,PLLEN,PLL5, NOCPUDIV, NOWDT,NOPROTECT // nicht ver?ndern
|
4 | #fuses ADC12, RTCOSC_INT, DSWDTOSC_T1 // nicht ver?ndern
|
5 | #use delay(clock=48000000) // nicht ver?ndern
|
6 | #use i2c(Master,sda=PIN_B5,scl=PIN_B4,fast=6400) //I2C eintrag
|
7 |
|
8 | // F?r Bootloader ------------------------------------------------
|
9 | #build (reset=0x1000, interrupt=0x1008) // neue Adressen // nicht ver?ndern
|
10 | #org 0x0,0xfff {} // res. Bereich // nicht ver?ndern
|
11 | // ---------------------------------------------------------------
|
12 |
|
13 |
|
14 | #include <usb_cdc.h>
|
15 |
|
16 | #define DS_write 0x20
|
17 | #define DS_read 0x21
|
18 |
|
19 | //#define forLeft set_pwm6_duty
|
20 | //#define forRight set_pwm7_duty
|
21 | //#define backLeft set_pwm9_duty
|
22 | //#define backRight set_pwm10_duty
|
23 |
|
24 | // schreibe Char in USB-Transmitt-Buffer
|
25 | //void usb_putc(char cc) // printf(usb_putc,...)
|
26 | //{
|
27 | // while(!usb_cdc_putready());
|
28 | // usb_cdc_putc(cc);
|
29 | //}
|
30 | // USB -------------------------------------------------------------------------------------------
|
31 |
|
32 |
|
33 | // globale Variable
|
34 |
|
35 | int1 toggle=0;
|
36 | int winkelHB, winkelLB, entfHB, entfLB;
|
37 | long winkel, dis;
|
38 | float div;
|
39 |
|
40 |
|
41 | //ISR Interrupt Srevice Routine TMR0
|
42 | #INT_TIMER0
|
43 | void tmr0_handler() //Name der Routine
|
44 | {
|
45 | set_timer0(18660); // da ja 1s
|
46 | toggle=1; // 1 -> 1s vorbei
|
47 | }
|
48 |
|
49 | void HW_init(){
|
50 |
|
51 | //SETUP_TIMER_2(T2_DIV_BY_1,255,1);
|
52 | //SETUP_CCP6(CCP_PWM);
|
53 | //SETUP_CCP7(CCP_PWM);
|
54 | //SETUP_CCP9(CCP_PWM);
|
55 | //SETUP_CCP10(CCP_PWM);
|
56 |
|
57 | i2c_start(); //start
|
58 | i2c_write(DS_write); //Adresse 0x20, ->Write=0, Read=1
|
59 | i2c_write(0x70); //Wake up
|
60 | i2c_stop(); //stop
|
61 |
|
62 | //Wait for wake up
|
63 | delay_ms(1100);
|
64 | }
|
65 |
|
66 |
|
67 | void doEntf(){ //Messvorgang in cm Starten
|
68 |
|
69 | i2c_start();
|
70 | i2c_write(0xE0); //slave id (0xE0) und write, LSB = low
|
71 | i2c_write(0);
|
72 | i2c_write(0x51); //Entfernung
|
73 | i2c_stop();
|
74 |
|
75 | delay_ms(65);
|
76 |
|
77 | i2c_start();
|
78 | i2c_write(0xE0); //slave id (0xE0) und write, LSB = low
|
79 | i2c_write(0x02); //von register 2 lesen (= entfernung high Byte)
|
80 | i2c_stop();
|
81 |
|
82 | i2c_start();
|
83 | i2c_write(0xE1); //slave id (0xE0) und READ, LSB = high
|
84 | entfHB = i2c_read(1); //do ACK
|
85 | entfLB = i2c_read(0); //don't ACK
|
86 | i2c_stop(); //stop
|
87 |
|
88 | delay_ms(10);
|
89 |
|
90 |
|
91 | dis = make16 (entfHB, entfLB);
|
92 | }
|
93 |
|
94 |
|
95 | void doCalibration(){
|
96 |
|
97 | i2c_start();
|
98 | i2c_write(DS_write);
|
99 | i2c_write(0xE0);
|
100 | i2c_stop();
|
101 | }
|
102 |
|
103 | //I2C Compass einlesen
|
104 | void doCompass(){
|
105 |
|
106 |
|
107 | i2c_start();
|
108 | i2c_write(DS_write);
|
109 | i2c_write(0xC0);
|
110 | i2c_stop();
|
111 |
|
112 | delay_ms(210);
|
113 |
|
114 | i2c_start();
|
115 | i2c_write(DS_read);
|
116 | winkelHB = i2c_read();
|
117 | winkelLB = i2c_read(0);
|
118 | i2c_stop();
|
119 |
|
120 | delay_ms(10);
|
121 | }
|
122 |
|
123 |
|
124 | void doFor(){
|
125 |
|
126 | output_high(Pin_B6);
|
127 | output_low(Pin_B7);
|
128 | output_high(Pin_C7);
|
129 | output_low(Pin_C6);
|
130 | }
|
131 |
|
132 | void doBack(){
|
133 |
|
134 |
|
135 | output_low(Pin_B6);
|
136 | output_low(Pin_C7);
|
137 | output_high(Pin_B6);
|
138 | output_high(Pin_C7);
|
139 | }
|
140 |
|
141 | void doRight(){
|
142 |
|
143 | output_low(Pin_B6);
|
144 | output_high(Pin_B7);
|
145 | output_low(Pin_C6);
|
146 | output_low(Pin_C7);
|
147 |
|
148 | }
|
149 |
|
150 | void doLeft(){
|
151 |
|
152 | output_low(Pin_B0);
|
153 | output_low(Pin_B1);
|
154 | output_high(Pin_B2);
|
155 | output_low(Pin_B3);
|
156 |
|
157 | }
|
158 |
|
159 | void doStop(){
|
160 |
|
161 | output_high(Pin_B6);
|
162 | output_high(Pin_B7);
|
163 | output_high(Pin_C6);
|
164 | output_high(Pin_C7);
|
165 |
|
166 | }
|
167 |
|
168 | // Hauptprogramm
|
169 | void main() {
|
170 |
|
171 | HW_init();
|
172 |
|
173 |
|
174 | while (true){
|
175 |
|
176 |
|
177 | if(input(PIN_A5)){ //Vorwärts
|
178 |
|
179 | delay_ms(2000);
|
180 |
|
181 | while(true){
|
182 |
|
183 | if(dis >= 20){
|
184 |
|
185 | doFor();
|
186 |
|
187 | doEntf();
|
188 |
|
189 | }
|
190 |
|
191 | else{
|
192 |
|
193 | doStop();
|
194 |
|
195 |
|
196 | }
|
197 | delay_ms(80);
|
198 |
|
199 | }
|
200 | }
|
201 | }
|
202 | }
|