Diff: main.cpp

Revision:

28:fd21faad6dd8

Parent:

27:24a9ac72fe92

Child:

29:f905e66e1a6f

--- a/main.cpp	Thu May 12 19:05:05 2016 +0000
+++ b/main.cpp	Fri May 13 09:26:13 2016 +0000
@@ -52,42 +52,28 @@
 //EncoderData enc6Data = {PB_5, PB_4, 102.083 * 64};
 //EncoderMotor m6(m6Data, enc6Data, speedPIDData, turnPIDData, NULL); 
 
-//const int mitu = 5;
-
-//EncoderMotor m[mitu] = {m1,m2,m3,m4,m5};//,m6};
-
-//SpeedEncoder s(encData);
-
-//PIDController ec(0.2, 0.1, 0.01);
-// PIDController ec(0.3, 2.0, 0.02);
-//PIDController ec(0.75, 2.0, 0.015);
-//PIDController ec(0.8, 1.5, 0.017);
-// PIDController ecRot(5.0, 0.1, 0.04);
-//EncoderMotor m(mData, encData, ec, ecRot);
-//Motor m(PB_0, PC_1, PC_0);
+const int MOTORS = 5;
+EncoderMotor* ms[MOTORS] = {&m1, &m2, &m3, &m4, &m5};//,m6};
+volatile int active = 0;
 
 Ticker ticker;
 
 void rise()
 {
     //pc.printf("rise\n");
-    m1.drive(0);
+    //m1.drive(0);
 }
 
 void fall()
 {
     //pc.printf("fall\n");
-    m1.drive(0.25);
+    //m1.drive(0.25);
 }
 
 void tick()
 {
-    m1.tick();
-    m2.tick();
-    m3.tick();
-    m4.tick();
-    m5.tick();
-    //m6.tick();
+    for (int i = 0; i < MOTORS; i++)
+        ms[i]->tick();
 }
 
 const float tickTime = 1.f / 60;
@@ -98,24 +84,29 @@
     bt.rise(&rise);
     bt.fall(&fall);
     
+    //ms[active]->drive(0.25);
+    
     ticker.attach(&tick, tickTime);
     
     float rot, speed;
-    //m.rotate(1.f, 0.5);
-    
-    //sync.update(NULL, 0.5);
     
     while(1)
     {
+        int oldactive = active;
+        scanf("%d", &active);
+        active--;
+        ms[oldactive]->drive(0);
+        ms[active]->drive(0.25);
+        //pc.printf("%d", active);
         //scanf("%f", &turn);
         //m.rotate(turn, 0.2);
-        scanf("%f %f", &rot, &speed);
-        m1.rotate(rot, speed);
+        //scanf("%f %f", &rot, &speed);
+        //m1.rotate(rot, speed);
         //m.drive(speed);
         //printf("%f %f\n", s.getTurnSpeed(), m.getSetSpeed());
         //printf("%f %f\n", m.s, m.getSetSpeed());
-        printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn());
+        //printf("%ld %f %f\n", m1.getEncoder().getCount(), m1.getEncoder().getTurn(), m1.getSetTurn());
         //printf("%f %f\n", m.getEncoder().getTurn(), m.getSetTurn());
-        wait(1.f / 60);
+        wait(1.f / 10);
     }
 }   
\ No newline at end of file