API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Jan 17th, 2017
105
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 12.85 KB | None | 0 0
raw download clone embed print report
  1. /*
  2. * EV3waySample.java (for leJOS EV3)
  3. * Created on: 2015/05/09
  4. * Author: INACHI Minoru
  5. * Copyright (c) 2015 Embedded Technology Software Design Robot Contest
  6. */
  7.  
  8. import java.io.DataInputStream;
  9. import java.io.IOException;
  10. import java.io.InputStream;
  11. import java.net.ServerSocket;
  12. import java.net.Socket;
  13. import java.util.Timer;
  14. import java.util.TimerTask;
  15.  
  16. import lejos.hardware.Battery;
  17. import lejos.hardware.lcd.LCD;
  18. import lejos.hardware.port.BasicMotorPort;
  19. import lejos.utility.Delay;
  20.  
  21. /**
  22. * Java sample program for leJOS EV3 of two-wheel inverted pendulum line trace robot.
  23. */
  24. public class demo {
  25. // The following parameters will need to be tuned to suit the individual sensor / environment
  26. private static final float GYRO_OFFSET = 0.0F; //gyro sensor offset value
  27. private static final float LIGHT_WHITE = 0.3F; // white color sensor brightness value
  28.  
  29. private static final float LIGHT_BLACK = 0.0F; // black color sensor brightness value
  30. private static final float SONAR_ALERT_DISTANCE = 0.3F; // obstacle detection distance by ultrasonic sensor [m]
  31. private static final int TAIL_ANGLE_STAND_UP = 94; // complete stop when the angle [degrees]
  32. private static final int TAIL_ANGLE_DRIVE = 3; // balance during running of the angle [degrees]
  33. private static final float P_GAIN = 2.5F; // complete stop motor control proportionality coefficient
  34. private static final int PWM_ABS_MAX = 60; // complete stop motor control PWM absolute maximum
  35. private static final int SOCKET_PORT = 7360; // port to be connected to the PC
  36. private static final int REMOTE_COMMAND_START = 71; // 'g'
  37. private static final int REMOTE_COMMAND_STOP = 83; // 's'
  38. private static final float THRESHOLD = (LIGHT_WHITE+LIGHT_BLACK)/2.0F; // line threshold of trace
  39. private static final int CNTLIMIT = 95;
  40. private static final float KP = 200.0F;
  41.  
  42. private static ServerSocket server = null;
  43. private static Socket client = null;
  44. private static InputStream inputStream = null;
  45. private static DataInputStream dataInputStream = null;
  46. private static int remoteCommand = 0;
  47.  
  48. private static EV3Body body = new EV3Body();
  49. private static int counter = 0;
  50. private static boolean alert = false; //Detect some range before found obstacles
  51. private static boolean aware = false; //Detect an obstacles
  52. private static boolean isRight = true;
  53. private static boolean isFront = false;
  54. private static int turncounter = 0;
  55. private static int stopcounter = 0;
  56. private static int uscounter = 0;
  57. private static int seekrange = 0;
  58. private static float lastdistance = 0;
  59.  
  60.  
  61. /**
  62. * Main
  63. */
  64. public static void main(String[] args) {
  65.  
  66. LCD.drawString("Please Wait... ", 0, 4);
  67. body.gyro.reset();
  68. body.sonar.enable();
  69. body.motorPortL.setPWMMode(BasicMotorPort.PWM_BRAKE);
  70. body.motorPortR.setPWMMode(BasicMotorPort.PWM_BRAKE);
  71. body.motorPortT.setPWMMode(BasicMotorPort.PWM_BRAKE);
  72.  
  73.  
  74. // Initial execution performance of Java is bad, it is not possible to obtain sufficient real-time to an inverted pendulum.
  75. // About the methods frequently used to traveling, to empty run until HotSpot is converted to native code.
  76. // Default number of executions that HotSpot occurs 1500.
  77. for (int i=0; i < 1500; i++) {
  78. body.motorPortL.controlMotor(0, 0);
  79. body.getBrightness();
  80. body.getSonarDistance();
  81. body.getGyroValue();
  82. Battery.getVoltageMilliVolt();
  83. Balancer.control(0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 8000);
  84. }
  85. Delay.msDelay(1000); // In another thread wait until the wonder time that HotSpot is complete.
  86.  
  87. body.motorPortL.controlMotor(0, 0);
  88. body.motorPortR.controlMotor(0, 0);
  89. body.motorPortT.controlMotor(0, 0);
  90. body.motorPortL.resetTachoCount(); // left motor encoder reset
  91. body.motorPortR.resetTachoCount(); // right motor encoder reset
  92. body.motorPortT.resetTachoCount(); // tail motor encoder reset
  93. Balancer.init(); // inverted pendulum control initialization
  94.  
  95.  
  96. // Remote connection
  97. Timer rcTimer = new Timer();
  98. TimerTask rcTask = new TimerTask() { // remote command task
  99. @Override
  100. public void run() {
  101. if (server == null) { // Not connected
  102. try {
  103. server = new ServerSocket(SOCKET_PORT);
  104. client = server.accept();
  105. inputStream = client.getInputStream();
  106. dataInputStream = new DataInputStream(inputStream);
  107. } catch (IOException ex) {
  108. ex.printStackTrace();
  109. server = null;
  110. dataInputStream = null;
  111. }
  112. } else {
  113. try {
  114. if (dataInputStream.available() > 0) {
  115. remoteCommand = dataInputStream.readInt();
  116. }
  117. } catch (IOException ex) {
  118. }
  119. }
  120. }
  121. };
  122. rcTimer.schedule(rcTask, 0, 20);
  123.  
  124. // Waiting for the start of
  125. LCD.drawString("Touch to START", 0, 4);
  126. boolean touchPressed = false;
  127. for (;;) {
  128. tailControl(body, TAIL_ANGLE_STAND_UP); //complete stop for angle control
  129. if (body.touchSensorIsPressed()) {
  130. touchPressed = true; // touch sensor is pressed
  131. } else {
  132. if (touchPressed) break; // touch sensor I was released after being pressed
  133. }
  134. if (checkRemoteCommand(REMOTE_COMMAND_START)) break; // in the PC 'g' key is pressed
  135. Delay.msDelay(20);
  136. }
  137.  
  138.  
  139. LCD.drawString("Running ", 0, 4);
  140. Timer driveTimer = new Timer();
  141. TimerTask driveTask = new TimerTask() {
  142.  
  143. @Override
  144. public void run() {
  145. //tailControl(body, TAIL_ANGLE_DRIVE); // balance for running angle to control
  146.  
  147. if (++counter >= 40/4) { // for each // about 40ms and obstacle detection
  148. alert = sonarAlert(body); // obstacle detection
  149. aware = sonarAware(body); // detection before found obstacle
  150. counter = 0;
  151. }
  152.  
  153. float forward = 0.0F; // forward-reverse instruction
  154. float turn = 0.0F; // turning instruction
  155. float distance = body.getSonarDistance();
  156. if(body.touchSensorIsPressed())stopcounter=300;
  157. if(distance < 0.50f)uscounter = 400;
  158.  
  159. if(stopcounter >= 0){//リセット後安定させるために少し停止(forword,turnの初期値を使う)
  160. stopcounter--;
  161. }
  162. else if(uscounter <= 0){
  163. if(isRight^isFront){//超音波センサーが感知していない時は回る
  164. turn = 40.0F;
  165. }
  166. else{
  167. turn = -40.0F;
  168. }
  169. if(++turncounter - seekrange * 50 > 250){
  170. if(isFront){
  171. isRight = !isRight;++seekrange;
  172. }
  173. isFront = !isFront;
  174. turncounter = 0;
  175. }
  176.  
  177. }
  178. else{//超音波センサーが感知してる時とその直後は前に動く
  179.  
  180.  
  181.  
  182. if(distance < 0.50F){
  183. lastdistance = distance;
  184. }
  185.  
  186. forward = KP*(lastdistance - 0.15F);
  187. if(forward < -40.0F){
  188.  
  189. forward = -40.0F;
  190. }
  191. else if(forward > 40.0F){
  192. forward =40.0F;
  193. }
  194. if(uscounter <= 200){//前に動いてから回るとき、その前に減速してその後の動作を安定させる
  195. forward *= 1 - uscounter/200;
  196. }
  197.  
  198. isFront = false;
  199. seekrange = 0;
  200. --uscounter;
  201. }
  202.  
  203.  
  204. /*if(stopcounter>0){
  205. forward = 0;
  206. turn = 0;
  207. stopcounter--;
  208. }*/
  209.  
  210. float gyroNow = body.getGyroValue(); // gyro sensor value
  211. int thetaL = body.motorPortL.getTachoCount(); // left motor rotation angle
  212. int thetaR = body.motorPortR.getTachoCount(); // right motor rotation angle
  213. int battery = Battery.getVoltageMilliVolt(); // battery voltage [mV]
  214. if(stopcounter>200){// タッチセンサーが押されている時とその直後は停止してしっぽを立てる
  215. seekrange = 0;
  216. body.motorPortL.resetTachoCount(); // left motor encoder reset
  217. body.motorPortR.resetTachoCount(); // right motor encoder reset
  218. Balancer.init(); // inverted pendulum control initialization
  219. Balancer.control (0, 0, gyroNow, GYRO_OFFSET, thetaL, thetaR, battery); // inverted pendulum control
  220. body.motorPortL.controlMotor(0, 1); // left motor PWM output set
  221. body.motorPortR.controlMotor(0, 1); // right motor PWM output set
  222. tailControl(body, TAIL_ANGLE_STAND_UP);
  223. }else{
  224. Balancer.control (forward, turn, gyroNow, GYRO_OFFSET, thetaL, thetaR, battery); // inverted pendulum control
  225. body.motorPortL.controlMotor(Balancer.getPwmL(), 1); // left motor PWM output set
  226. body.motorPortR.controlMotor(Balancer.getPwmR(), 1); // right motor PWM output set
  227. tailControl(body, TAIL_ANGLE_DRIVE);
  228. }
  229.  
  230. }
  231. };
  232. driveTimer.scheduleAtFixedRate(driveTask, 0, 4);
  233.  
  234. for (;;) {
  235.  
  236. /*if(body.touchSensorIsPressed()){
  237. rcTimer.cancel();
  238. driveTimer.cancel();
  239. Delay.msDelay(3000);
  240. rcTimer.schedule(rcTask, 0, 20);
  241. driveTimer.scheduleAtFixedRate(driveTask, 0, 4);
  242.  
  243.  
  244. }*/
  245.  
  246. }
  247.  
  248. }
  249.  
  250. /*
  251. * The obstacle detection by the ultrasonic sensor
  252. * @return true(there is an obstacle) / false (no obstacle))
  253. */
  254. private static final boolean sonarAlert(EV3Body body) {
  255. float distance = body.getSonarDistance();
  256. if ((distance <= SONAR_ALERT_DISTANCE) && (distance >= 0)) {
  257. return true; // obstacle; return true
  258. }
  259. return false;
  260. }
  261.  
  262. /*
  263. * The checking gray color point by sheck point that is not black color
  264. * @return true(not black color) / false (black color))
  265. */
  266. private static final boolean checkGrayzone(EV3Body body){
  267. float value = body.getBrightness();
  268. if(value >= 0.06 && value<=0.14){
  269. return true;
  270. }else{
  271. return false;
  272. }
  273. }
  274. private static final boolean checkNotBlackzone(EV3Body body){
  275. float value = body.getBrightness();
  276. if(value > 0.05){
  277. return true;
  278. }else{
  279. return false;
  280. }
  281. }
  282.  
  283. /*
  284. * The obstacle awareness by the ultrasonic sensor
  285. * @return true(there is an obstacle) / false (no obstacle))
  286. */
  287. private static final boolean sonarAware(EV3Body body) {
  288. float distance = body.getSonarDistance();
  289. if ((distance <= SONAR_ALERT_DISTANCE+0.2F) && (distance >= SONAR_ALERT_DISTANCE)) {
  290. return true; // obstacle; return true
  291. }
  292. return false;
  293. }
  294.  
  295. /*
  296. * Traveling body full stop motor of angle control
  297. * @param angle motor target angle [degree]
  298. */
  299. private static final void tailControl(EV3Body body, int angle) {
  300. float pwm = (float)(angle - body.motorPortT.getTachoCount()) * P_GAIN; // proportional control
  301. // PWM output saturation processing
  302. if (pwm > PWM_ABS_MAX) {
  303. pwm = PWM_ABS_MAX;
  304. } else if (pwm < -PWM_ABS_MAX) {
  305. pwm = -PWM_ABS_MAX;
  306. }
  307. body.motorPortT.controlMotor((int)pwm, 1);
  308. }
  309.  
  310. /*
  311. * Check the remote command
  312. */
  313. private static final boolean checkRemoteCommand(int command) {
  314. if (remoteCommand > 0) {
  315. if (remoteCommand == command) {
  316. return true;
  317. }
  318. }
  319. return false;
  320. }
  321. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!