for hanzo4.1

arduino/ServoSetHanzo4_1/ServoSetHanzo4_1.ino

@@ -0,0 +1,123 @@
+#include <Servo.h>
+
+// Servo : SC-1267SG
+unsigned int steeringServoCenter = 1820;  //us
+unsigned int steeringServoMin = (steeringServoCenter-780); //us
+unsigned int steeringServoMax = (steeringServoCenter+780); //us
+
+// Servo : SC-0352
+unsigned int lanceServoCenter = 1560;  //us
+unsigned int lanceServoMin = (lanceServoCenter-780); //us
+unsigned int lanceServoMax = (lanceServoCenter+780); //us
+
+unsigned char rotPin     = 2;
+
+unsigned char numSensors = 8;
+unsigned char sensorPin[] = {19,18,17,16,15,14,13,12};  //右端から順番に左端へ
+
+unsigned char steeringServoPin  =  9;
+unsigned char lanceServoPin     = 10;
+
+volatile unsigned long distance = 0;
+
+Servo lanceServo;
+Servo steeringServo;
+
+/* ステアリングを中央からa[度]だけ動かす
+  <- - 0 + ->
+      ||
+      ||
+ []---{}---[]
+     / .\
+*/
+int steering(int a){
+  steeringServo.write(90 + a);
+  return (steeringServo.read() - 90);
+}
+
+/* ランスを中央からa[度]だけ動かす
+  <- - 0 + ->
+     / .\
+    /  | \
+ []+--{}--+[]
+*/
+int lance(int a){
+  lanceServo.write(90 - a);
+  return (90 - a);
+}
+
+/* センサーのアレイの初期化 */
+void sensorInit(){
+  for(int i=0; i<numSensors; i++){
+    pinMode(sensorPin[i], INPUT);
+  }
+}
+
+/* エンコーダーの割り込み処理 */
+void rot(){
+  distance++;
+}
+
+void setup() {
+  steeringServo.attach(steeringServoPin, steeringServoMin, steeringServoMax);
+  lanceServo.attach(lanceServoPin, lanceServoMin, lanceServoMax);
+  steering(0);
+  lance(0);
+  pinMode(rotPin, INPUT);
+  attachInterrupt(0, rot, RISING);
+  Serial.begin(9600);
+  Serial.println("format : [+-][0-180][hl]");
+}
+
+void loop() {
+  static int i = 0;
+  i++;
+  if(i>100){
+    i = 0;
+    Serial.print(distance);
+    Serial.print('\t');
+    for(int i=0; i<numSensors; i++){
+      Serial.print(digitalRead(sensorPin[i]));
+    }
+    Serial.println();
+  }
+  
+  static int v = 0;
+  static int s = 1;
+  
+  if ( Serial.available() ) {
+    char ch = Serial.read();
+    switch(ch) {
+      case '-':
+        s = -1;
+        break;
+      case '0'...'9':
+        v = v * 10 + ch - '0';
+        break;
+      case 'l':
+        v = s*v;
+        Serial.print("lance :");
+        Serial.println(v);
+        lance(v);
+        v = 0;
+        s = 1;
+        break;
+      case 'h':
+        v = s*v;
+        Serial.print("steering :");
+        Serial.println(v);
+        steering(v);
+        v = 0;
+        s = 1;
+        break;
+      case 'c':
+        Serial.println("clear");
+        distance = 0;
+        v = 0;
+        s = 1;
+        break;
+    }
+  }
+  
+  delay(1);
+}