svnno****@sourc*****
svnno****@sourc*****
2011年 7月 24日 (日) 18:16:27 JST
Revision: 14
http://sourceforge.jp/projects/andy/svn/view?view=rev&revision=14
Author: yishii
Date: 2011-07-24 18:16:26 +0900 (Sun, 24 Jul 2011)
Log Message:
-----------
andy tiger 1
Added Paths:
-----------
trunk/tank_with_2_servomotors_AndyTiger1/
trunk/tank_with_2_servomotors_AndyTiger1/tank_with_servo.pde
Removed Paths:
-------------
trunk/tank_with_2_servomotors_AndyTiger1/tank_with_servo.pde
-------------- next part --------------
Deleted: trunk/tank_with_2_servomotors_AndyTiger1/tank_with_servo.pde
===================================================================
--- trunk/tank_with_2_servomotors/tank_with_servo.pde 2011-04-19 10:59:19 UTC (rev 9)
+++ trunk/tank_with_2_servomotors_AndyTiger1/tank_with_servo.pde 2011-07-24 09:16:26 UTC (rev 14)
@@ -1,304 +0,0 @@
-//
-// Andy 2 with 2 servomotors
-//
-// Coded by Yasuhiro ISHII
-//
-
-#include <MsTimer2.h>
-#include <Servo.h>
-
-#define PIN_MOTOR_R_VREF 5
-#define PIN_MOTOR_L_VREF 6
-#define PIN_MOTOR_R_CONTROL1 8
-#define PIN_MOTOR_R_CONTROL2 9
-#define PIN_MOTOR_L_CONTROL1 10
-#define PIN_MOTOR_L_CONTROL2 11
-
-#define MOTORTYPE_MOTOR_R 0
-#define MOTORTYPE_MOTOR_L 1
-
-#define MOTORACTION_MOTOR_STOP 0
-#define MOTORACTION_MOTOR_FORWARD 1
-#define MOTORACTION_MOTOR_REVERSE 2
-#define MOTORACTION_MOTOR_BRAKE 3
-
-#define MESSAGE_ACK 0x30
-#define MESSAGE_ERROR 0x31
-#define MESSAGE_NACK 0x32
-
-#define MESSAGE_CATEGORY_INDICATOR_LED 0x10
-#define MESSAGE_CATEGORY_CATERPILLAR 0x20
-#define MESSAGE_CATEGORY_SERVOMOTOR 0x21
-#define MESSAGE_CATEGORY_UNKNOWN -1
-
-#define SERIAL_MESSAGE_RECEIVE_MAX_LENGTH 100 /* Serial message max receive length [byte(s)] */
-#define CYCLIC_HANDLER_INTERVAL_MS 100
-#define SERIAL_COMMUNICATION_BAUD_RATE 115200
-#define SERIAL_MESSAGE_RECEIVE_TIMEOUT_100MS 10 /* Timeout time for receiving command (10*100[ms]) */
-
-typedef enum SERIAL_RECEIVE_STATE {
- SRECV_IDLE,
- SRECV_RECEIVING,
- SRECV_ERROR
-} SerialReceiveState;
-
-signed char motorPower_L;
-signed char motorPower_R;
-int serial_timeout_timer = 0;
-int serial_timeout_detect = 0;
-Servo servomotor_L;
-Servo servomotor_R;
-
-void setup()
-{
- /////////////////////////////////////////////////
- // GPIO SETUP
- /////////////////////////////////////////////////
-
- // pin direction setup
- pinMode(2,OUTPUT);
- pinMode(3,OUTPUT);
- pinMode(4,OUTPUT);
- pinMode(5,OUTPUT); // Vref (R ch)
- pinMode(6,OUTPUT); // Vref (L ch)
- pinMode(7,OUTPUT);
-
- pinMode(8,OUTPUT); // IN1 (Rch)
- pinMode(9,OUTPUT); // IN2 (Rch)
- pinMode(10,OUTPUT); // IN1 (Rch)
- pinMode(11,OUTPUT); // IN2 (Rch)
-
- pinMode(12,OUTPUT);
- pinMode(13,OUTPUT);
-
- // pin output value setup
- digitalWrite(8,LOW);
- digitalWrite(9,LOW);
- digitalWrite(10,LOW);
- digitalWrite(11,LOW);
-
-
- /////////////////////////////////////////////////
- // MISC SETUP
- /////////////////////////////////////////////////
-
- // initialize peripherals
- analogWrite(PIN_MOTOR_R_VREF,0);
- analogWrite(PIN_MOTOR_L_VREF,0);
-
- motorPower_L = 0;
- motorPower_R = 0;
-
- // initialize servo motor
-
- servomotor_L.attach(12,800,2200);
- servomotor_R.attach(13,800,2200);
-
- // start cyclic handler
- MsTimer2::set(CYCLIC_HANDLER_INTERVAL_MS, cyclic_handler);
- MsTimer2::start();
-
- // start UART
- Serial.begin(SERIAL_COMMUNICATION_BAUD_RATE);
- Serial.flush();
-}
-
-void loop()
-{
- static SerialReceiveState serial_receive_state = SRECV_IDLE;
- static int length = 0;
- static int category = -1;
- static int current_position = 0;
- static unsigned char command_buffer[SERIAL_MESSAGE_RECEIVE_MAX_LENGTH+1];
- int tmodet;
-
- noInterrupts();
- {
- tmodet = serial_timeout_detect;
- }
- interrupts();
-
- switch(serial_receive_state){
- case SRECV_IDLE:
- if(Serial.available() > 0){
- length = Serial.read();
- if(length < SERIAL_MESSAGE_RECEIVE_MAX_LENGTH){
- serial_receive_state = SRECV_RECEIVING;
- category = MESSAGE_CATEGORY_UNKNOWN;
- noInterrupts();
- {
- serial_timeout_timer = SERIAL_MESSAGE_RECEIVE_TIMEOUT_100MS;
- serial_timeout_detect = 0;
- }
- interrupts();
- current_position = 0;
- } else {
- Serial.write(MESSAGE_ERROR);
- }
- }
- break;
- case SRECV_RECEIVING:
- if(tmodet){
- serial_receive_state = SRECV_ERROR;
- } else {
- if(Serial.available() > 0){
- {
- unsigned char readdata;
- readdata = Serial.read();
- if(current_position == 0){
- category = readdata;
- } else {
- command_buffer[current_position] = readdata;
- }
- }
-
- current_position++;
- if(current_position == length){
-
- noInterrupts();
- {
- serial_timeout_timer = 0;
- }
- interrupts();
-
- switch(category){
- case MESSAGE_CATEGORY_CATERPILLAR:
- noInterrupts();
- {
- // process command
- motorPower_L = command_buffer[1];
- motorPower_R = command_buffer[2];
- }
- interrupts();
-
- // return ACK to the host
- Serial.write(MESSAGE_ACK);
-
- break;
- case MESSAGE_CATEGORY_SERVOMOTOR:
- {
- unsigned char motor[2];
- motor[0] = command_buffer[1];
- motor[1] = command_buffer[2];
- motor[0] = motor[0] < 1 ? 1 : motor[0];
- motor[0] = motor[0] >= 180 ? 180 : motor[0];
- motor[1] = motor[1] < 1 ? 1 : motor[1];
- motor[1] = motor[1] >= 180 ? 180 : motor[1];
- motor[1] = 181 - motor[1];
- servomotor_L.write(motor[0]);
- servomotor_R.write(motor[1]);
- Serial.write(MESSAGE_ACK);
- }
- break;
- case MESSAGE_CATEGORY_INDICATOR_LED:
- digitalWrite(13,command_buffer[1] == 0 ? LOW : HIGH);
-
- // return ACK to the host
- Serial.write(MESSAGE_ACK);
- break;
- default:
- // command with unknown category is received
-
- Serial.write(MESSAGE_NACK);
- break;
- }
- serial_receive_state = SRECV_IDLE;
- }
- }
- }
-
- break;
- case SRECV_ERROR:
- Serial.write(MESSAGE_ERROR);
- category = MESSAGE_CATEGORY_UNKNOWN;
- serial_receive_state = SRECV_IDLE;
-
- break;
- default:
- break;
- }
-
-}
-
-// for Debug
-void led_indicator(void)
-{
- static int flag = 0;
-
- if(flag){
- digitalWrite(13,LOW);
- flag = 0;
- } else {
- digitalWrite(13,HIGH);
- flag = 1;
- }
-}
-
-void serial_watcher(void)
-{
- if(serial_timeout_timer != 0){
- serial_timeout_timer--;
- if(serial_timeout_timer == 0){
- serial_timeout_detect = 1;
- motorPower_L = 0;
- motorPower_R = 0;
- }
- }
-}
-
-void setMotorSpeed(int power_l,int power_r)
-{
- analogWrite(PIN_MOTOR_L_VREF,power_l);
- analogWrite(PIN_MOTOR_R_VREF,power_r);
- delay(10);
-}
-
-void controlMotor(int motortype,int motoraction)
-{
- int pinno_1;
- int pinno_2;
-
- const unsigned char cont[4][2] = {
- { LOW , LOW }, // STOP
- { HIGH , LOW }, // FORWARD
- { LOW , HIGH }, // REVERSE
- { HIGH , HIGH } // BRAKE
- };
-
- if(motortype == MOTORTYPE_MOTOR_R){
- pinno_1 = PIN_MOTOR_R_CONTROL1;
- pinno_2 = PIN_MOTOR_R_CONTROL2;
- } else {
- pinno_1 = PIN_MOTOR_L_CONTROL1;
- pinno_2 = PIN_MOTOR_L_CONTROL2;
- }
-
- digitalWrite(pinno_1,cont[motoraction][0]);
- digitalWrite(pinno_2,cont[motoraction][1]);
-}
-
-void controlMotorProcessMain(void)
-{
- // motorPower_L,motorPower_R : -128(rev max) ～ 0(stop) ～ +127(fwd max)
- int dirMotor_L;
- int dirMotor_R;
-
- if(!motorPower_L && !motorPower_R){
- setMotorSpeed(255,255);
- controlMotor(MOTORTYPE_MOTOR_R,MOTORACTION_MOTOR_BRAKE);
- controlMotor(MOTORTYPE_MOTOR_L,MOTORACTION_MOTOR_BRAKE);
- } else {
- dirMotor_L = motorPower_L < 0 ? MOTORACTION_MOTOR_REVERSE : MOTORACTION_MOTOR_FORWARD;
- dirMotor_R = motorPower_R < 0 ? MOTORACTION_MOTOR_REVERSE : MOTORACTION_MOTOR_FORWARD;
-
- controlMotor(MOTORTYPE_MOTOR_R,dirMotor_R);
- controlMotor(MOTORTYPE_MOTOR_L,dirMotor_L);
- setMotorSpeed(abs(motorPower_L) << 1,abs(motorPower_R) << 1);
- }
-}
-
-void cyclic_handler(void)
-{
- serial_watcher();
- controlMotorProcessMain();
-}
Copied: trunk/tank_with_2_servomotors_AndyTiger1/tank_with_servo.pde (from rev 11, trunk/tank_with_2_servomotors/tank_with_servo.pde)
===================================================================
--- trunk/tank_with_2_servomotors_AndyTiger1/tank_with_servo.pde (rev 0)
+++ trunk/tank_with_2_servomotors_AndyTiger1/tank_with_servo.pde 2011-07-24 09:16:26 UTC (rev 14)
@@ -0,0 +1,321 @@
+//
+// Andy 2 with 2 servomotors v0.2
+//
+// Coded by Yasuhiro ISHII
+//
+// 0.2 : Andy Shield supported
+
+#include <MsTimer2.h>
+#include <Servo.h>
+
+#define ANDY_SHIELD 1 // Andy Shield rev.1
+
+#ifdef ANDY_SHIELD
+#define PIN_MOTOR_L_VREF 6
+#define PIN_MOTOR_R_VREF 5
+#define PIN_MOTOR_L_CONTROL1 A0
+#define PIN_MOTOR_L_CONTROL2 A1
+#define PIN_MOTOR_R_CONTROL1 A3
+#define PIN_MOTOR_R_CONTROL2 A2
+#else
+#define PIN_MOTOR_R_VREF 5
+#define PIN_MOTOR_L_VREF 6
+#define PIN_MOTOR_R_CONTROL1 8
+#define PIN_MOTOR_R_CONTROL2 9
+#define PIN_MOTOR_L_CONTROL1 10
+#define PIN_MOTOR_L_CONTROL2 11
+#endif
+
+
+#define MOTORTYPE_MOTOR_R 0
+#define MOTORTYPE_MOTOR_L 1
+
+#define MOTORACTION_MOTOR_STOP 0
+#define MOTORACTION_MOTOR_FORWARD 1
+#define MOTORACTION_MOTOR_REVERSE 2
+#define MOTORACTION_MOTOR_BRAKE 3
+
+#define MESSAGE_ACK 0x30
+#define MESSAGE_ERROR 0x31
+#define MESSAGE_NACK 0x32
+
+#define MESSAGE_CATEGORY_INDICATOR_LED 0x10
+#define MESSAGE_CATEGORY_CATERPILLAR 0x20
+#define MESSAGE_CATEGORY_SERVOMOTOR 0x21
+#define MESSAGE_CATEGORY_UNKNOWN -1
+
+#define SERIAL_MESSAGE_RECEIVE_MAX_LENGTH 100 /* Serial message max receive length [byte(s)] */
+#define CYCLIC_HANDLER_INTERVAL_MS 100
+#define SERIAL_COMMUNICATION_BAUD_RATE 9600
+#define SERIAL_MESSAGE_RECEIVE_TIMEOUT_100MS 10 /* Timeout time for receiving command (10*100[ms]) */
+
+typedef enum SERIAL_RECEIVE_STATE {
+ SRECV_IDLE,
+ SRECV_RECEIVING,
+ SRECV_ERROR
+} SerialReceiveState;
+
+signed char motorPower_L;
+signed char motorPower_R;
+int serial_timeout_timer = 0;
+int serial_timeout_detect = 0;
+Servo servomotor_L;
+Servo servomotor_R;
+
+void setup()
+{
+ /////////////////////////////////////////////////
+ // GPIO SETUP
+ /////////////////////////////////////////////////
+
+ // hardware bug fix
+//#if ANDY_SHIELD==1
+ // only rev.1 has the mis-port assignment
+ pinMode(4,INPUT);
+ digitalWrite(4,LOW);
+//#endif
+
+ // pin direction setup
+ pinMode(PIN_MOTOR_R_VREF,OUTPUT);
+ pinMode(PIN_MOTOR_L_VREF,OUTPUT);
+ pinMode(PIN_MOTOR_R_CONTROL1,OUTPUT);
+ pinMode(PIN_MOTOR_R_CONTROL2,OUTPUT);
+ pinMode(PIN_MOTOR_L_CONTROL1,OUTPUT);
+ pinMode(PIN_MOTOR_L_CONTROL2,OUTPUT);
+
+ // pin output value setup
+ digitalWrite(PIN_MOTOR_R_CONTROL1,LOW);
+ digitalWrite(PIN_MOTOR_R_CONTROL2,HIGH);
+ digitalWrite(PIN_MOTOR_L_CONTROL1,LOW);
+ digitalWrite(PIN_MOTOR_L_CONTROL2,HIGH);
+ analogWrite(PIN_MOTOR_R_VREF,100);
+ analogWrite(PIN_MOTOR_L_VREF,100);
+// while(1);
+ /////////////////////////////////////////////////
+ // MISC SETUP
+ /////////////////////////////////////////////////
+
+ // initialize peripherals
+ analogWrite(PIN_MOTOR_R_VREF,0);
+ analogWrite(PIN_MOTOR_L_VREF,0);
+
+ motorPower_L = 0;
+ motorPower_R = 0;
+
+ // initialize servo motor
+
+#ifndef ANDY_SHIELD
+ servomotor_L.attach(12,800,2200);
+ servomotor_R.attach(13,800,2200);
+#endif
+
+ // start cyclic handler
+ MsTimer2::set(CYCLIC_HANDLER_INTERVAL_MS, cyclic_handler);
+ MsTimer2::start();
+
+ // start UART
+ Serial.begin(SERIAL_COMMUNICATION_BAUD_RATE);
+ Serial.flush();
+}
+
+void loop()
+{
+ static SerialReceiveState serial_receive_state = SRECV_IDLE;
+ static int length = 0;
+ static int category = -1;
+ static int current_position = 0;
+ static unsigned char command_buffer[SERIAL_MESSAGE_RECEIVE_MAX_LENGTH+1];
+ int tmodet;
+
+ noInterrupts();
+ {
+ tmodet = serial_timeout_detect;
+ }
+ interrupts();
+
+ switch(serial_receive_state){
+ case SRECV_IDLE:
+ if(Serial.available() > 0){
+ length = Serial.read();
+ if(length < SERIAL_MESSAGE_RECEIVE_MAX_LENGTH){
+ serial_receive_state = SRECV_RECEIVING;
+ category = MESSAGE_CATEGORY_UNKNOWN;
+ noInterrupts();
+ {
+ serial_timeout_timer = SERIAL_MESSAGE_RECEIVE_TIMEOUT_100MS;
+ serial_timeout_detect = 0;
+ }
+ interrupts();
+ current_position = 0;
+ } else {
+ Serial.write(MESSAGE_ERROR);
+ }
+ }
+ break;
+ case SRECV_RECEIVING:
+ if(tmodet){
+ serial_receive_state = SRECV_ERROR;
+ } else {
+ if(Serial.available() > 0){
+ {
+ unsigned char readdata;
+ readdata = Serial.read();
+ if(current_position == 0){
+ category = readdata;
+ } else {
+ command_buffer[current_position] = readdata;
+ }
+ }
+
+ current_position++;
+ if(current_position == length){
+
+ noInterrupts();
+ {
+ serial_timeout_timer = 0;
+ }
+ interrupts();
+
+ switch(category){
+ case MESSAGE_CATEGORY_CATERPILLAR:
+ noInterrupts();
+ {
+ // process command
+ motorPower_L = command_buffer[1];
+ motorPower_R = command_buffer[2];
+ }
+ interrupts();
+
+ // return ACK to the host
+ Serial.write(MESSAGE_ACK);
+
+ break;
+ case MESSAGE_CATEGORY_SERVOMOTOR:
+ {
+ unsigned char motor[2];
+ motor[0] = command_buffer[1];
+ motor[1] = command_buffer[2];
+ motor[0] = motor[0] < 1 ? 1 : motor[0];
+ motor[0] = motor[0] >= 180 ? 180 : motor[0];
+ motor[1] = motor[1] < 1 ? 1 : motor[1];
+ motor[1] = motor[1] >= 180 ? 180 : motor[1];
+ motor[1] = 181 - motor[1];
+#ifndef ANDY_SHIELD
+ servomotor_L.write(motor[0]);
+ servomotor_R.write(motor[1]);
+#endif
+ Serial.write(MESSAGE_ACK);
+ }
+ break;
+ case MESSAGE_CATEGORY_INDICATOR_LED:
+ digitalWrite(13,command_buffer[1] == 0 ? LOW : HIGH);
+
+ // return ACK to the host
+ Serial.write(MESSAGE_ACK);
+ break;
+ default:
+ // command with unknown category is received
+
+ Serial.write(MESSAGE_NACK);
+ break;
+ }
+ serial_receive_state = SRECV_IDLE;
+ }
+ }
+ }
+
+ break;
+ case SRECV_ERROR:
+ Serial.write(MESSAGE_ERROR);
+ category = MESSAGE_CATEGORY_UNKNOWN;
+ serial_receive_state = SRECV_IDLE;
+
+ break;
+ default:
+ break;
+ }
+
+}
+
+// for Debug
+void led_indicator(void)
+{
+ static int flag = 0;
+
+ if(flag){
+ digitalWrite(13,LOW);
+ flag = 0;
+ } else {
+ digitalWrite(13,HIGH);
+ flag = 1;
+ }
+}
+
+void serial_watcher(void)
+{
+ if(serial_timeout_timer != 0){
+ serial_timeout_timer--;
+ if(serial_timeout_timer == 0){
+ serial_timeout_detect = 1;
+ motorPower_L = 0;
+ motorPower_R = 0;
+ }
+ }
+}
+
+void setMotorSpeed(int power_l,int power_r)
+{
+ analogWrite(PIN_MOTOR_L_VREF,power_l);
+ analogWrite(PIN_MOTOR_R_VREF,power_r);
+ delay(10);
+}
+
+void controlMotor(int motortype,int motoraction)
+{
+ int pinno_1;
+ int pinno_2;
+
+ const unsigned char cont[4][2] = {
+ { LOW , LOW }, // STOP
+ { HIGH , LOW }, // FORWARD
+ { LOW , HIGH }, // REVERSE
+ { HIGH , HIGH } // BRAKE
+ };
+
+ if(motortype == MOTORTYPE_MOTOR_R){
+ pinno_1 = PIN_MOTOR_R_CONTROL1;
+ pinno_2 = PIN_MOTOR_R_CONTROL2;
+ } else {
+ pinno_1 = PIN_MOTOR_L_CONTROL1;
+ pinno_2 = PIN_MOTOR_L_CONTROL2;
+ }
+
+ digitalWrite(pinno_1,cont[motoraction][0]);
+ digitalWrite(pinno_2,cont[motoraction][1]);
+}
+
+void controlMotorProcessMain(void)
+{
+ // motorPower_L,motorPower_R : -128(rev max) ～ 0(stop) ～ +127(fwd max)
+ int dirMotor_L;
+ int dirMotor_R;
+
+ if(!motorPower_L && !motorPower_R){
+ setMotorSpeed(255,255);
+ controlMotor(MOTORTYPE_MOTOR_R,MOTORACTION_MOTOR_BRAKE);
+ controlMotor(MOTORTYPE_MOTOR_L,MOTORACTION_MOTOR_BRAKE);
+ } else {
+ dirMotor_L = motorPower_L < 0 ? MOTORACTION_MOTOR_REVERSE : MOTORACTION_MOTOR_FORWARD;
+ dirMotor_R = motorPower_R < 0 ? MOTORACTION_MOTOR_REVERSE : MOTORACTION_MOTOR_FORWARD;
+
+ controlMotor(MOTORTYPE_MOTOR_R,dirMotor_R);
+ controlMotor(MOTORTYPE_MOTOR_L,dirMotor_L);
+ setMotorSpeed(abs(motorPower_L) << 1,abs(motorPower_R) << 1);
+ }
+}
+
+void cyclic_handler(void)
+{
+ serial_watcher();
+ controlMotorProcessMain();
+}