svnno****@sourc*****
svnno****@sourc*****
2011年 7月 24日 (日) 19:54:10 JST
Revision: 15
http://sourceforge.jp/projects/andy/svn/view?view=rev&revision=15
Author: yishii
Date: 2011-07-24 19:54:10 +0900 (Sun, 24 Jul 2011)
Log Message:
-----------
Andy Tiger1 firmware
Added Paths:
-----------
trunk/tank_with_2_servomotors_AndyTiger1/tank_with_2_servomotors_AndyTiger1.pde
-------------- next part --------------
Added: trunk/tank_with_2_servomotors_AndyTiger1/tank_with_2_servomotors_AndyTiger1.pde
===================================================================
--- trunk/tank_with_2_servomotors_AndyTiger1/tank_with_2_servomotors_AndyTiger1.pde (rev 0)
+++ trunk/tank_with_2_servomotors_AndyTiger1/tank_with_2_servomotors_AndyTiger1.pde 2011-07-24 10:54:10 UTC (rev 15)
@@ -0,0 +1,370 @@
+//
+// 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 MOTORTYPE_MOTOR_FORT 2
+
+#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_FORTMOTOR 0x22
+#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;
+signed char fortMotorPower;
+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);
+
+ pinMode(A0,OUTPUT);
+ pinMode(A1,OUTPUT);
+ pinMode(A2,OUTPUT);
+ pinMode(A3,OUTPUT);
+ pinMode(A4,OUTPUT);
+ pinMode(A5,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_FORTMOTOR:
+ noInterrupts();
+ {
+ fortMotorPower = command_buffer[1];
+ }
+ interrupts();
+ // return ACK to the host
+ 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)
+{
+
+ if(power_l > 0){
+ controlMotor(MOTORTYPE_MOTOR_L,1);
+ } else if(power_l < 0){
+ controlMotor(MOTORTYPE_MOTOR_L,2);
+ } else if(power_l == 0){
+ controlMotor(MOTORTYPE_MOTOR_L,0);
+ }
+ if(power_r > 0){
+ controlMotor(MOTORTYPE_MOTOR_R,1);
+ } else if(power_r < 0){
+ controlMotor(MOTORTYPE_MOTOR_R,2);
+ } else if(power_r == 0){
+ controlMotor(MOTORTYPE_MOTOR_R,0);
+ }
+
+}
+
+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
+ { LOW , LOW } // BRAKE
+ };
+
+ switch(motortype){
+ case MOTORTYPE_MOTOR_R:
+ pinno_1 = A0;
+ pinno_2 = A1;
+ break;
+ case MOTORTYPE_MOTOR_L:
+ pinno_1 = A2;
+ pinno_2 = A3;
+ break;
+ case MOTORTYPE_MOTOR_FORT:
+ pinno_1 = A4;
+ pinno_2 = A5;
+ break;
+ default:
+ break;
+ }
+ 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);
+
+ }
+ if(fortMotorPower == 0){
+ controlMotor(MOTORTYPE_MOTOR_FORT,MOTORACTION_MOTOR_BRAKE);
+ } else if(fortMotorPower > 0){
+ controlMotor(MOTORTYPE_MOTOR_FORT,MOTORACTION_MOTOR_FORWARD);
+ } else {
+ controlMotor(MOTORTYPE_MOTOR_FORT,MOTORACTION_MOTOR_REVERSE);
+ }
+}
+
+void cyclic_handler(void)
+{
+ serial_watcher();
+ controlMotorProcessMain();
+}