/********* TKB49 つくれば工房 第二回講習会 番外編 当日デモ走行をさせていたタミヤの工作基本セットを用いて作ったライントレースカー ******/ /* Test of Motor Driver TB6612FNG Dual Motor Driver by POLOLU ******* 2013/9/26 I. Endo ******/ /** defined a new function void driving(float velocity,float stearing) ** combied with reflection sensors **** 2013.10.4 **/ /* keep going while the sensor value is unchanged*/ int LED=13; int AIN1=4; int AIN2=2; int PWMA=3; int STBY=5; int BIN1=7; int BIN2=8; int PWMB=9; int sensorA=0; int sensorB=1; int threshA=800; int threshB=800; int sensed=0; int ledpinA=10; int ledpinB=11; int s=3; int s0=3; void setup(){ pinMode(LED,OUTPUT); pinMode(AIN1,OUTPUT); pinMode(AIN2,OUTPUT); pinMode(PWMA,OUTPUT); pinMode(BIN1,OUTPUT); pinMode(BIN2,OUTPUT); pinMode(PWMB,OUTPUT); pinMode(STBY,OUTPUT); pinMode(ledpinA,OUTPUT); pinMode(ledpinB,OUTPUT); digitalWrite(STBY,HIGH); Serial.begin(9600); } void loop(){ switch(s){ case 3:driving(255,0); break; case 1:driving(255,50); break; case 2:driving(255,-50); break; case 0:if ((s0=0) || (s0=3)) driving(-255,0) ; else{if( s0=1) driving(-255,100); if (s0=2) driving(-255,-100);}; break; } delay(10); do {s=sensors(700,700);} while (s==s0); s0=s; } void driving(float velocity,float stearing){ //stearing should be between -100 and 100 with positive/negative values to turn to the right/left. // velocity should be between -255 and 255 with positive/negative values to advance to the foward/backward. int vL,vR; if(stearing>=0){ vL=int(velocity); vR=int(velocity*(1-stearing/50));} else{ vL=int(velocity*(1+stearing/50)); vR=int(velocity);}; Amotor(vL); Bmotor(vR); } void Amotor(int v){ if( v>0){ digitalWrite(AIN1,HIGH); digitalWrite(AIN2,LOW);} else{ digitalWrite(AIN1,LOW); digitalWrite(AIN2,HIGH); v=-v;} analogWrite(PWMA,v); } void Bmotor(int v){ if( v>0){ digitalWrite(BIN1,HIGH); digitalWrite(BIN2,LOW);} else{ digitalWrite(BIN1,LOW); digitalWrite(BIN2,HIGH); v=-v;} analogWrite(PWMB,v); } void BFoward(int v){ digitalWrite(BIN1,HIGH); digitalWrite(BIN2,LOW); analogWrite(PWMB,v); } void Free(){ digitalWrite(AIN1,HIGH); digitalWrite(AIN2,HIGH); digitalWrite(BIN1,HIGH); digitalWrite(BIN2,HIGH); } void Brake(){ digitalWrite(AIN1,LOW); digitalWrite(AIN2,LOW); digitalWrite(BIN1,LOW); digitalWrite(BIN2,LOW); } int sensors(int thA,int thB){ int s1; int s2; int ss; if (analogRead(sensorA)>thA) {s1=1;digitalWrite(ledpinA,HIGH);} else {s1=0;digitalWrite(ledpinA,LOW);}; if (analogRead(sensorB)>thB) {s2=2;digitalWrite(ledpinB,HIGH);} else {s2=0;digitalWrite(ledpinB,LOW);}; ss=s1+s2; //for debug // Serial.print(ss); // Serial.print("\t sensorA = "); // Serial.print(analogRead(sensorA)); // Serial.print("\t sensorB = "); // Serial.println(analogRead(sensorB)); // return ss; }