sampleProgram

Dependencies:   QEI accelerator bit_test cyclic_io cyclic_var cylinder event_var limit mbed mecanum motor_drive pid pid_encoder rs422_put sbdbt servo

Fork of 17robo_fuzi by kusano kiyoshige

Revision:
24:da0fc94add43
Parent:
23:aeff5fd36ae1
Child:
25:124aee8a7742
--- a/main.cpp	Sat Aug 05 04:23:30 2017 +0000
+++ b/main.cpp	Sun Aug 06 02:01:23 2017 +0000
@@ -42,6 +42,7 @@
 #define enc_Kp      0.01
 #define enc_Ki      0.01
 #define enc_Kd      0.01
+#define powerdown   0.6
 
 DigitalOut led1(LED1);
 //DigitalOut led2(LED2);
@@ -122,11 +123,11 @@
 
     switch (counter) {
         case 0:
-            rs422.put(id[counter], v1.duty(mecanum.v1()), v3.duty(mecanum.v3()));
+            rs422.put(id[counter], v1.duty(mecanum.v1()*powerdown), v3.duty(mecanum.v3()*powerdown));
             counter++;
             break;
         case 1:
-            rs422.put(id[counter], v2.duty(mecanum.v2()), v4.duty(mecanum.v4()));
+            rs422.put(id[counter], v2.duty(mecanum.v2()*powerdown), v4.duty(mecanum.v4()*powerdown));
             counter ++;
             break;
         case 2:
@@ -134,11 +135,11 @@
             counter ++;
             break;
         case 3:
-            rs422.put(id[counter], (sbdbt.sankaku*0.5-sbdbt.batu*0.5), (sbdbt.up*0.5-sbdbt.down*0.5) );
+            rs422.put(id[counter], (-sbdbt.sankaku*0.8+sbdbt.batu*0.8), (sbdbt.up*0.8-sbdbt.down*0.8));
             counter ++;
             break;
         case 4:
-            rs422.put(id[counter], ((float)sword_left.getState()),0.0);
+            rs422.put(id[counter], ((float)sword_right.getState()*0.8),((float)sword_left.getState()*0.8));
             counter = 0;
             break;
         default: