axispipe.py - Opengist

修訂 3c106b4ff360a5d89f64cb26fad348d6520cb7c0

axispipe_328.ino · 2.2 KiB · Arduino 原始檔案

#include <Servo.h> Servo ch[6]; unsigned char buff[3]; unsigned short count; unsigned short kcount = 0; void TurnOn() { ch[0].attach(3); ch[1].attach(5); ch[2].attach(6); ch[3].attach(9); ch[4].attach(8); ch[5].attach(10); digitalWrite(13, HIGH); Serial.println("Controller ON"); } void TurnOff() { for(int i=0;i<6;i++) ch[i].detach(); digitalWrite(3, LOW); digitalWrite(5, LOW); digitalWrite(6, LOW); digitalWrite(8, LOW); digitalWrite(9, LOW); digitalWrite(10, LOW); digitalWrite(13, LOW); Serial.println("Controller OFF"); } void KeepAlive() { TCCR2B = 0x00; TCNT2 = 0x00; TIFR2 = 0x00; TIMSK2 = 0x01; TCCR2A = 0x00; TCCR2B |= (1<< CS12) | (1<< CS10); kcount = 0; } ISR(TIMER2_OVF_vect) { //We didnt receive the signal yet. So we must shutdown the output kcount++; if(kcount == 255) TurnOff(); TCCR2B = 0x00; TCNT2 = 0x00; TIFR2 = 0x00; TIMSK2 = 0x01; TCCR2A = 0x00; TCCR2B |= (1<< CS12) | (1<< CS10); } void setup() { Serial.begin(38400); Serial.println("Inititializing controllers"); pinMode(13, OUTPUT); pinMode(3, OUTPUT); pinMode(5, OUTPUT); pinMode(6, OUTPUT); pinMode(8, OUTPUT); pinMode(9, OUTPUT); pinMode(10, OUTPUT); digitalWrite(3, LOW); digitalWrite(5, LOW); digitalWrite(6, LOW); digitalWrite(8, LOW); digitalWrite(9, LOW); digitalWrite(10, LOW); count = 0; Serial.println("AxisPipe Started!"); KeepAlive(); } void loop() { short val; if(count == 3) { count = 0; val = buff[1] + buff[2] * 256; switch(buff[0]) { case 0: case 1: case 2: case 3: ch[buff[0]].writeMicroseconds(val); break; case 4: break; // Nothing on 328 case 5: TurnOn(); break; case 6: TurnOff(); break; case 7: case 8: // Aux Outputs ch[buff[0]-3].writeMicroseconds(val); break; case 9: break; default: Serial.println("Unknown CMD"); } KeepAlive(); } if (Serial.available() > 0) { buff[count] = Serial.read(); count++; } }

1	#include <Servo.h>
2
3	Servo ch[6];
4
5	unsigned char buff[3];
6	unsigned short count;
7	unsigned short kcount = 0;
8	void TurnOn() {
9	ch[0].attach(3);
10	ch[1].attach(5);
11	ch[2].attach(6);
12	ch[3].attach(9);
13	ch[4].attach(8);
14	ch[5].attach(10);
15	digitalWrite(13, HIGH);
16	Serial.println("Controller ON");
17	}
18
19	void TurnOff() {
20	for(int i=0;i<6;i++)
21	ch[i].detach();
22	digitalWrite(3, LOW);
23	digitalWrite(5, LOW);
24	digitalWrite(6, LOW);
25	digitalWrite(8, LOW);
26	digitalWrite(9, LOW);
27	digitalWrite(10, LOW);
28	digitalWrite(13, LOW);
29	Serial.println("Controller OFF");
30	}
31
32	void KeepAlive() {
33	TCCR2B = 0x00;
34	TCNT2 = 0x00;
35	TIFR2 = 0x00;
36	TIMSK2 = 0x01;
37	TCCR2A = 0x00;
38	TCCR2B \|= (1<< CS12) \| (1<< CS10);
39	kcount = 0;
40	}
41
42	ISR(TIMER2_OVF_vect) {
43	//We didnt receive the signal yet. So we must shutdown the output
44	kcount++;
45	if(kcount == 255)
46	TurnOff();
47	TCCR2B = 0x00;
48	TCNT2 = 0x00;
49	TIFR2 = 0x00;
50	TIMSK2 = 0x01;
51	TCCR2A = 0x00;
52	TCCR2B \|= (1<< CS12) \| (1<< CS10);
53	}
54
55	void setup() {
56	Serial.begin(38400);
57	Serial.println("Inititializing controllers");
58	pinMode(13, OUTPUT);
59	pinMode(3, OUTPUT);
60	pinMode(5, OUTPUT);
61	pinMode(6, OUTPUT);
62	pinMode(8, OUTPUT);
63	pinMode(9, OUTPUT);
64	pinMode(10, OUTPUT);
65
66	digitalWrite(3, LOW);
67	digitalWrite(5, LOW);
68	digitalWrite(6, LOW);
69	digitalWrite(8, LOW);
70	digitalWrite(9, LOW);
71	digitalWrite(10, LOW);
72
73	count = 0;
74	Serial.println("AxisPipe Started!");
75	KeepAlive();
76	}
77
78	void loop() {
79	short val;
80	if(count == 3) {
81	count = 0;
82	val = buff[1] + buff[2] * 256;
83	switch(buff[0]) {
84	case 0:
85	case 1:
86	case 2:
87	case 3:
88	ch[buff[0]].writeMicroseconds(val);
89	break;
90	case 4: break; // Nothing on 328
91	case 5: TurnOn(); break;
92	case 6: TurnOff(); break;
93	case 7:
94	case 8: // Aux Outputs
95	ch[buff[0]-3].writeMicroseconds(val);
96	break;
97	case 9: break;
98	default:
99	Serial.println("Unknown CMD");
100	}
101	KeepAlive();
102	}
103	if (Serial.available() > 0) {
104	buff[count] = Serial.read();
105	count++;
106	}
107
108	}