Diff: main_ps3.cpp

Revision:

0:bd4719e15f7e

Child:

1:a89e2a604dcf

diff -r 000000000000 -r bd4719e15f7e main_ps3.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_ps3.cpp	Sun Aug 30 10:49:16 2015 +0000
@@ -0,0 +1,136 @@
+/**
+ * This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
+ */
+    
+#include "machine_ps3.h"
+
+Serial pc(USBTX, USBRX);
+
+//LocalFileSystem local("local");
+
+int main() {
+    
+    /*FILE *fp_r = fopen("/local/out_r.dat", "w");
+    FILE *fp_l = fopen("/local/out_l.dat", "w");
+    FILE *fp_d = fopen("/local/out_d.dat", "w");*/
+    
+    initializeSBDBT();
+    initializeMotors();
+    initializeControllers();
+    
+    targ_move_r_velocity=0.0;
+    targ_move_l_velocity=0.0;
+    
+    while(1) {
+        
+        if(up){
+            //move_r_controller.setTunings(2.0,10.0,0.0);
+            //move_l_controller.setTunings(2.0,10.0,0.0);
+            move_r_controller.reset();
+            move_l_controller.reset();
+            targ_move_r_velocity=50.0;
+            targ_move_l_velocity=50.0;
+        }
+        else if(right){
+            move_r_controller.reset();
+            move_l_controller.reset();
+<<<<<<< local
+            targ_move_r_velocity=-50.0;
+            targ_move_l_velocity=0.0;
+        } else if(down) {
+=======
+            targ_move_r_velocity=0.0;
+            targ_move_l_velocity=50.0;
+        }
+        else if(down){
+>>>>>>> other
+            //move_r_controller.setTunings(2.0,10.0,0.0);
+            //move_l_controller.setTunings(2.0,10.0,0.0);
+            move_r_controller.reset();
+            move_l_controller.reset();
+            targ_move_r_velocity=-50.0;
+            targ_move_l_velocity=-50.0;
+        }
+        else if(left){
+            move_r_controller.reset();
+            move_l_controller.reset();
+            targ_move_r_velocity=50.0;
+            targ_move_l_velocity=0.0;
+<<<<<<< local
+        } else if(r1) {
+            move_r_controller.reset();
+            move_l_controller.reset();
+=======
+        }
+        else if(r1){
+>>>>>>> other
+            targ_move_r_velocity=-50.0;
+            targ_move_l_velocity=50.0;
+<<<<<<< local
+        } else if(l1) {
+            move_r_controller.reset();
+            move_l_controller.reset();
+=======
+        }
+        else if(l1){
+>>>>>>> other
+            targ_move_r_velocity=50.0;
+            targ_move_l_velocity=-50.0;
+<<<<<<< local
+        } else if(r2) {
+            move_r_controller.reset();
+            move_l_controller.reset();
+            targ_move_r_velocity=0.0;
+            targ_move_l_velocity=50.0;
+        } else if(l2) {
+            move_r_controller.reset();
+            move_l_controller.reset();
+            targ_move_r_velocity=50.0;
+            targ_move_l_velocity=0.0;
+        } else if(cross) {
+=======
+        }
+        else if(cross){
+>>>>>>> other
+            targ_move_r_velocity=0.0;
+            targ_move_l_velocity=0.0;
+            wait(0.5);
+            move_r_controller.reset();
+            move_l_controller.reset();
+            /*fclose(fp_r);
+            fclose(fp_l);
+            fclose(fp_d);*/
+<<<<<<< local
+        } else if(square) {
+            move_r_controller.reset();
+            move_l_controller.reset();
+=======
+        }
+        else if(square){
+>>>>>>> other
+            targ_move_r_velocity=0.0;
+            targ_move_l_velocity=0.0;
+        }
+        else if(circle){
+            Emergency_toggle();
+        }
+        else if(r1){
+            RS485.putc(1);
+            wait(1);
+            RS485.putc(3);
+        }
+        mesure_move_r_velocity();
+        mesure_move_l_velocity();
+        calculate_diff();
+        Move_r_speed_following();
+        Move_l_speed_following();
+        //pc.printf("targ is:%ld ,velocity is:%ld ,cont is %f\r\n", targ_swing_velocity, swing_velocity, cont_swing);
+        pc.printf("velocity r is:%f velocity l is:%f\r\n",move_r_velocity,move_l_velocity);
+        //pc.printf("compute r is:%f compute l is:%f\r\n",cont_move_r,cont_move_l);
+        //pc.printf("targ r is:%ld ,targ l is:%ld\r\n",targ_move_r_velocity,targ_move_l_velocity);
+        /*fprintf(fp_r, "%ld\r\n",move_r_velocity);
+        fprintf(fp_l, "%ld\r\n",move_l_velocity);
+        fprintf(fp_d, "%ld\r\n",diff);*/
+        wait(RATE);
+    }
+}
\ No newline at end of file