shinnosuke hanamura
a
Dependencies: MotorDriver_SU mbed
Diff: c.cpp
- Revision:
- 0:a421b8dde3ab
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/c.cpp Thu Dec 04 06:49:04 2014 +0000
@@ -0,0 +1,162 @@
+#include "mbed.h"
+#include "MotorDriver_SU.h"
+
+DigitalIn sw(P0_1, PullUp);
+// trace L to R
+DigitalIn sensor1(P0_16);
+DigitalIn sensor2(P0_13);
+DigitalIn sensor3(P0_11);
+// lift L to R
+DigitalIn sensor4(P0_21);
+DigitalIn sensor6(P0_14);
+DigitalIn sensor5(P0_22);
+DigitalIn sensor7(P0_23);
+
+DigitalOut led(P1_28);
+
+MotorDriver_SU motor(MOTOR_DOUBLE);
+MotorDriver_SU lift(MOTOR_SINGLE);
+
+int main(){
+ while(1){
+
+ //int height = 0;//lift position
+ int check1 = 0;//convert count
+ /*int check2 = 0;*/
+ int count = 0;//count liftsensor B&B
+ while(sw == 0){
+ motor.Drive(0, STOP, 0); //R
+ motor.Drive(1, STOP, 0);//L
+ lift.Drive(0, STOP, 0);
+ }
+//////////////////////////////////////////////////////////
+ //need to change some numerical value after test
+//////////////////////////////////////////////////////////
+ while(sw == 1){
+ if(sensor1 == 1 && sensor2 == 1 && sensor3 == 0){
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 1500);
+ }
+ else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1){
+ motor.Drive(0, CW, 2500);
+ motor.Drive(1, CW, 2500);
+ lift.Drive(0, STOP, 0);
+ }
+ else if((sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0)/*||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 0 && sensor7 == 0)||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor6 == 0 && sensor5 == 0)||(sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor5 == 0 && sensor7 == 0)*/){
+ led = 1;//count kakuninn
+ if( count == 0){
+ lift.Drive(0, CW, 4095); //lift up
+ motor.Drive(0, CW, 1300); //restart
+ motor.Drive(1, CW, 1300);
+ /*motor.Drive(0, STOP, 0); //stop
+ motor.Drive(1, STOP, 0);*/
+ wait(1.6);
+
+ led = 0;//count kakuninn
+ /* motor.Drive(0, CW, 2000); //restart
+ motor.Drive(1, CW, 2000);
+ lift.Drive(0, CW, 4095); //lift up more
+ wait(0.7);*/
+ lift.Drive(0, STOP, 0);
+ //height = 1;//lift position high
+ count++;
+ }
+ //need to change count[1,2,3...] after test
+ else if(count == 1){
+ count = 3;
+ }
+ else if(count == 3){
+ motor.Drive(0, STOP, 0);
+ motor.Drive(1, STOP, 0);
+ lift.Drive(0, CCW, 3000);//lift down
+ wait(1.2);
+ motor.Drive(0, CCW, 4095);//back for 2 seconds
+ motor.Drive(1, CCW, 4095);
+ lift.Drive(0, STOP, 0);
+ led = 0;//count kakunin
+ wait(2);
+ //height = 0;//lift position low
+ count = 0;//reset
+ }
+ else{
+ ;
+ }
+ }
+ else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 1){
+ motor.Drive(0, CW, 1500);
+ motor.Drive(1, CW, 500);
+ }
+ else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 1){
+ motor.Drive(0, CW, 1875);
+ motor.Drive(1, CW, 1500);
+ }
+ else if(sensor1 == 1 && sensor2 == 0 && sensor3 == 0){
+ motor.Drive(0, CW, 1500);
+ motor.Drive(1, CW, 1875);
+ }
+ else if(sensor1 == 0 && sensor2 == 1 && sensor3 == 0){
+ motor.Drive(0, CW, 500);//ccw change cw
+ motor.Drive(1, CW, 500);//ccw change cw
+ }
+ /*else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1){
+ motor.Drive(0, CW, 2048);
+ motor.Drive(1, STOP, 0);
+ lift.Drive(0, STOP, 0);
+ }*/
+ //all body on line's outside
+ else if(sensor1 == 1 && sensor2 == 1 && sensor3 == 1 ){
+ motor.Drive(0, CCW, 200);
+ motor.Drive(1, CW, 2000);
+ lift.Drive(0, STOP, 0);
+ }
+ //select course condition
+ else if((sensor2 == 0 && sensor4 == 1 && sensor5 == 0 )||(sensor2 == 0 && sensor4 == 1 && sensor7 == 0 )||(sensor2 == 0 && sensor6 == 1 && sensor5 == 0)||(sensor2 == 0 && sensor6 == 1 && sensor7 == 0)){
+ if(check1 == 0){
+ check1 = 1;
+ motor.Drive(0, CW, 1500);//adjust
+ motor.Drive(1, CW, 1500);
+ wait(0.5);//adjust
+ //enter the right courses
+ motor.Drive(0, CW, 500);//change both speed
+ motor.Drive(1, CW, 2500);
+ wait(1);
+ motor.Drive(0, CW, 1500);//adjust
+ motor.Drive(1, CW, 1500);
+ wait(0.15);//adjust
+ motor.Drive(0, CW, 1200);//change both speed
+ motor.Drive(1, CCW, 200);
+ wait(0.6);
+ /*motor.Drive(0, CW, 1500);//adjust
+ motor.Drive(1, CW, 1500);
+ wait(0.1);//adjust*/
+ }
+ else{
+ ;//response only once
+ }
+ }
+ /*else if((sensor2 == 0 && sensor4 == 0 && sensor5 == 1)||(sensor2 == 0 && sensor4 == 0 && sensor7 == 1)||(sensor2 == 0 && sensor6 == 0 && sensor5 == 1)||(sensor2 == 0 && sensor6 == 0 && sensor7 == 1)){
+ if(check2 == 0){
+ check2 = 1;
+ //enter the center course
+ motor.Drive(0, CW, 800);//change both speed
+ motor.Drive(1, CCW, 500);
+ wait(1);
+ }
+ else if(sensor1 == 0 && sensor2 == 0 && sensor3 == 0 ) {
+ motor.Drive(0, CW, 500);
+ motor.Drive(1, CW, 500);
+ }//tuika
+
+
+ else{
+ ;//response only once
+ }
+ }
+ */
+ else{
+ motor.Drive(0, CW, 2500);//spin to search correct way
+ motor.Drive(1, CW, 500);
+ }
+ }
+ }
+}
\ No newline at end of file