Diff: main.cpp

Revision:

2:cc875033357d

Parent:

1:e4fe1b3b7376

Child:

3:7fe433d24ff7

--- a/main.cpp	Tue May 21 07:12:26 2019 +0000
+++ b/main.cpp	Thu May 23 00:39:11 2019 +0000
@@ -156,8 +156,9 @@
 // interface to pc
 Serial pc(USBTX, USBRX);
 
-// motor
-MotorWithEncoder motor(p26, p27, 50, p5, p6, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
+// motor1
+MotorWithEncoder motor0(p21, p27, 50, p5, p6, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
+MotorWithEncoder motor1(p22, p28, 50, p7, p8, 64, QEI::X4_ENCODING, 10, 0.001, 0.001);
 // gear ratio: 50, encoder resolution: 64, control period: 10 m, Kp = 0.001, Ki = 0.001
 
 // key input from pc (callback)
@@ -165,30 +166,50 @@
     if (pc.readable() == 1) {
         switch(pc.getc()) {
             case 'z':
-                motor.switchControllerEnable(true);
+                motor0.switchControllerEnable(true);
                 break;
             case 'q':
-                motor.switchControllerEnable(false);
+                motor0.switchControllerEnable(false);
                 break;
             case 'a':
-                motor.setDesEncPulseRate(motor.getDesEncPulseRate() + 10);
+                motor0.setDesEncPulseRate(motor0.getDesEncPulseRate() + 10);
                 break;
             case 's':
-                motor.setDesEncPulseRate(0);
+                motor0.setDesEncPulseRate(0);
                 break;
             case 'd':
-                motor.setDesEncPulseRate(motor.getDesEncPulseRate() - 10);
+                motor0.setDesEncPulseRate(motor0.getDesEncPulseRate() - 10);
+            case 'v':
+                motor1.switchControllerEnable(true);
+                break;
+            case 'r':
+                motor1.switchControllerEnable(false);
+                break;
+            case 'f':
+                motor1.setDesEncPulseRate(motor1.getDesEncPulseRate() + 10);
+                break;
+            case 'g':
+                motor1.setDesEncPulseRate(0);
+                break;
+            case 'h':
+                motor1.setDesEncPulseRate(motor1.getDesEncPulseRate() - 10);
         }
     }
 }
 
 // encoder output to pc
 void enc_output_pc() {
-    if (motor.getControllerEnable()) {
-        pc.printf("ON: e = %d, d = %d, m = %f \n", motor.getEncPulseRate(), motor.getDesEncPulseRate(), motor.getInputVoltage());
+    if (motor0.getControllerEnable()) {
+        pc.printf("ON0: e = %d, d = %d, m = %.2f, ", motor0.getEncPulseRate(), motor0.getDesEncPulseRate(), motor0.getInputVoltage());
     }
     else {
-        pc.printf("OFF: e = %d, d = %d, m = %f \n", motor.getEncPulseRate(), motor.getDesEncPulseRate(), motor.getInputVoltage());
+        pc.printf("OFF0: e = %d, d = %d, m = %.2f, ", motor0.getEncPulseRate(), motor0.getDesEncPulseRate(), motor0.getInputVoltage());
+    }
+    if (motor1.getControllerEnable()) {
+        pc.printf("ON1: e = %d, d = %d, m = %.2f\n", motor1.getEncPulseRate(), motor1.getDesEncPulseRate(), motor1.getInputVoltage());
+    }
+    else {
+        pc.printf("OFF1: e = %d, d = %d, m = %.2f\n", motor1.getEncPulseRate(), motor1.getDesEncPulseRate(), motor1.getInputVoltage());
     }
 }
 
@@ -199,10 +220,13 @@
 // update motor voltage
 // update enc_pulse_ratge
 void callbackControlPeriod() {
-    motor.updateEncPulseRate();
-    motor.applyInputVoltage();
+    motor0.updateEncPulseRate();
+    motor1.updateEncPulseRate();
+    motor0.applyInputVoltage();
+    motor1.applyInputVoltage();
     
-    motor.updateInputVoltage();
+    motor0.updateInputVoltage();
+    motor1.updateInputVoltage();
 }
 
 // main() runs in its own thread in the OS
@@ -210,7 +234,8 @@
     pc.baud(9600);
     
     // event queue
-    queue.call_every(motor.getControlPeriod(), &callbackControlPeriod);
+    queue.call_every(motor0.getControlPeriod(), &callbackControlPeriod);
+    queue.call_every(motor1.getControlPeriod(), &callbackControlPeriod);
     queue.call_every(100, &key_input_pc);
     queue.call_every(100, &enc_output_pc);
     queue.dispatch();