import utmech.mechanompu.actuator.*; import utmech.mechanompu.io.*; import java.io.*; public class WheelPos extends wheels implements Runnable { private static long l_goal, r_goal; private static long l_cur=0, r_cur=0; private static int l_gain=10000, r_gain=10000; /* /1000 pwm val per error count */ private static WheelPos ptr; private static Thread t; public static final int dpr = 297; /* distance (mm) per round */ public static final int cpr = 552; /* count per round */ /* some cars have different value */ public static void init() { try { ptr=new WheelPos(); t=new Thread(ptr); t.start(); } catch (Exception e){ System.out.print(e); } } public void run() { PrintStream pw = new PrintStream(serial.out); set_servo_mode(false); int l_cont, r_cont; while(true) { l_cur=-getencl(); r_cur=getencr(); l_cont=(int)((-l_gain*(l_cur-l_goal))/1000); r_cont=(int)((-r_gain*(r_cur-r_goal))/1000); pw.println("l_cur"+l_cur+"r_cur"+r_cur+"l_cont"+l_cont+"r_cont"+r_cont+"l[mm]:"+l()+"r[mm]:"+r()); setpwml(-l_cont); setpwmr(-r_cont); try { Thread.sleep(5); } catch(InterruptedException ex) { System.out.println(ex); } } } public static long l(long val) { l_goal=val*cpr/dpr; return l_cur*dpr/cpr; } public static long r(long val) { r_goal=val*cpr/dpr; return r_cur*dpr/cpr; } public static long l() { return l_cur*dpr/cpr; } public static long r() { return r_cur*dpr/cpr; } public static int l_gain(int val) { l_gain = val; return l_gain; } public static int r_gain(int val) { r_gain = val; return r_gain; } public static int l_gain() { return l_gain; } public static int r_gain() { return r_gain; } }