1   package simworldobjects;
2   
3   import interfaces.*;
4   import main.*;
5   
6   import java.awt.*;
7   import java.lang.*;
8   import java.awt.event.*;
9   import javax.swing.*;
10  import java.io.*;
11  import java.util.*;
12  import java.awt.geom.*;
13  
14  import java.util.*;
15  import josx.util.*;
16  
17  /**
18  * Simulates an RCX for use in SimWorld
19  *
20  * @author Graham Ritchie
21  */
22  public class SimRCX extends BasicSimObject implements AbstractRobot 
23  {
24      public static final int STATE_STOPPED=1;
25      public static final int STATE_FORWARD=2;
26      public static final int STATE_BACKWARD=3;
27      public static final int STATE_RIGHT=4;
28      public static final int STATE_LEFT=5;
29          
30          private Point2D basePointQ1;
31          private Point2D basePointQ2;
32          private Point2D basePointQ3;
33          private Point2D basePointQ4;
34          
35          private static final double ANGLE_OFFSET = 28.18;
36          private static final double RADIUS = Math.sqrt((Math.pow(28,2)+Math.pow(15,2)));
37          private boolean rotateAboutWheel=false;
38          private int activeMotor = 3;
39      
40      private int state;
41      private SimWorld world;
42          private boolean selected = false;
43          private boolean activation = false;
44      
45      private SimSensor S1;
46      private SimSensor S2;
47      private SimSensor S3;
48      
49      private Controller controller;
50          private int MOTOR_POWER=1;
51          private int[] sensors;
52          private int BATTERY_VOLTAGE;
53          private String LCD_OUTPUT="";
54          
55          private double lastXCoord=0;
56          private double lastZCoord=0;
57          private boolean changeBack = false;
58          
59          private double decreasingAngle = 0;
60          
61          private Vector timerList = new  Vector();
62          private Sound soundPlayer=null;
63      
64      /**
65      * Constructor: sets up the robot with some predefined parameters
66      *
67      * @param w the SimWorld where this SimRCX resides
68      * @param c the Controller of this SimRCX
69      */
70      public SimRCX(SimWorld w, Controller c)
71      {
72          // initialise the SimObject
73          super(30.0,40.0,60.0,"robot",600.0,0.0,200.0,0.0,160.0);
74              
75                  // set this robot's world
76          this.world=w;
77          
78          // set this robot's controler
79          this.controller=c;
80                  
81                  //sets the robots battery voltage
82                  this.BATTERY_VOLTAGE=7900;
83          
84          setupRobot();
85                  
86      }
87      
88      /**
89      * Constructor: sets up the robot according to the parameters
90      */
91      public SimRCX(SimWorld w, Controller c, double height, double width, double length, String type, double x, double y, double z, double bearingXY, double bearingXZ, int battery)
92      {
93          // initialise the SimObject
94          super(height,width,length,type,x,y,z,bearingXY,bearingXZ);
95          
96          // set this robot's world
97          world=w;
98          
99          // set this robot's controler
100         this.controller=c;
101                 
102                 //sets the robots battery voltage
103                 this.BATTERY_VOLTAGE=battery;
104                 
105         setupRobot();
106     }
107     
108     /**
109     * Sets up robot: adds sensors etc.
110     */
111     private void setupRobot()
112     {
113         // set the robot's initial state to stopped
114         this.state=SimRCX.STATE_STOPPED;
115         
116         // set up sensors as specified in controller
117         sensors=controller.getSensors();
118         
119         if(sensors[0]==Controller.SENSOR_TYPE_LIGHT)
120         {
121             S1=new SimLightSensor(this,-(this.getWidth()/2)-7,-((this.getLength()/10)-10),'L');
122         }
123         else
124         {
125             S1=new SimTouchSensor(this,-this.getWidth(),-((this.getLength()/6)),'L');
126         }
127         
128         if(sensors[1]==Controller.SENSOR_TYPE_LIGHT)
129         {
130             S2=new SimLightSensor(this,0.0,-(this.getLength()/2)-25,'F'); 
131         }
132         else
133         {
134             S2=new SimTouchSensor(this,0.0,-(this.getLength()/2)-15,'F');
135         }
136         
137         if(sensors[2]==Controller.SENSOR_TYPE_LIGHT)
138         {
139             S3=new SimLightSensor(this,(this.getWidth()/2)+7,-((this.getLength()/10)-10),'R');
140         }
141         else
142         {
143             S3=new SimTouchSensor(this,this.getWidth(),-this.getLength()/4,'R');
144         }
145         
146         // add sensors to the world
147         world.addObject(S1);
148         world.addObject(S2);
149         world.addObject(S3);
150                                 
151     }
152         
153         public boolean getSelected(){
154             return this.selected;
155         }
156         public void setSelected(boolean selected){
157             this.selected = selected;
158         }
159     
160     public SimWorld getWorld()
161     {
162         return this.world;
163     }
164     
165     /*******************************************************************
166                      Methods required by AbstractRobot
167     *******************************************************************/
168     
169     /**
170     * Sets the robot moving forwards, this will continue until some other 
171     * method is called to stop it.
172     */
173     public void forward()
174     {
175             
176             this.rotateAboutWheel = false;
177             stopMoving();
178             this.state=SimRCX.STATE_FORWARD;
179             this.setDesiredVelocity(MOTOR_POWER);
180     }
181     
182     /**
183     * Makes the robot move forwards for the given amount of time
184     *
185     * @param time the time in milliseconds
186     */
187     public void forward(int time)
188     {
189             this.rotateAboutWheel = false;
190             stopMoving();
191             this.state=SimRCX.STATE_FORWARD;
192             this.setDesiredVelocity(MOTOR_POWER);
193             try{Thread.sleep(time);}catch(Exception e){}
194             stopMoving();
195     }
196     
197     /**
198     * Sets the robot moving backwards, this will continue until some other 
199     * method is called to stop it.
200     */
201     public void backward()
202     {
203             this.rotateAboutWheel = false;
204             stopMoving();
205             this.state=SimRCX.STATE_BACKWARD;
206             this.setDesiredVelocity(-(MOTOR_POWER));
207     }
208     
209     /**
210     * Makes the robot move backwards for the given amount of time
211     *
212     * @param time the time in milliseconds
213     */
214     public void backward(int time)
215     {
216             this.rotateAboutWheel = false;
217             stopMoving();
218             this.state=SimRCX.STATE_BACKWARD;
219             this.setDesiredVelocity(-(MOTOR_POWER));
220             try{Thread.sleep(time);}catch(Exception e){}
221             stopMoving();
222     }
223     
224     /**
225     * Sets the robot spinning right, this will continue until some other 
226     * method is called to stop it.
227     */
228     public void right()
229     {
230             this.rotateAboutWheel = false;
231             stopMoving();
232             this.state=SimRCX.STATE_RIGHT;
233             this.setDesiredBearingVelocityXZ(MOTOR_POWER);
234     }
235     
236     /**
237     * Spins the robot right for the given amount of time
238     *
239     * @param time the time in milliseconds
240     */
241     public void right(int time)
242     {
243             this.rotateAboutWheel = false;
244             stopMoving();
245             this.state=SimRCX.STATE_RIGHT;
246             this.setDesiredBearingVelocityXZ(MOTOR_POWER);
247             try{Thread.sleep(time);}catch(Exception e){}
248             stopMoving();
249     
250     }
251     
252     /**
253     * Sets the robot spinning left, this will continue until some other 
254     * method is called to stop it.
255     */
256     public void left()
257     {
258             this.rotateAboutWheel = false;
259             stopMoving();
260             this.state=SimRCX.STATE_LEFT;
261             this.setDesiredBearingVelocityXZ(-(MOTOR_POWER));
262     }
263     
264     /**
265     * Spins the robot left for the given amount of time
266     *
267     * @param time the time in milliseconds
268     */
269     public void left(int time)
270     {
271             this.rotateAboutWheel = false;
272             stopMoving();
273             this.state=SimRCX.STATE_LEFT;
274             this.setDesiredBearingVelocityXZ(-(MOTOR_POWER));
275             try{Thread.sleep(time);}catch(Exception e){}
276             stopMoving();
277     }
278         
279         /**
280     * Sets a single Motor moving, this will continue until some other 
281     * method is called to stop it
282         *
283     * @param motor the motor number (AbstractRobot.MOTOR_A or AbstractRobot.MOTOR_C);
284         * @param direction, the direction number (AbstractRobot.BACKWARDS or AbstractRobot.FORWARDS
285     */
286         public void singleMotor(int motor, int direction) {
287             
288             this.rotateAboutWheel = false;
289             stopMoving();
290             
291             this.activeMotor = motor;
292             this.makeQuadrantBasePts(this.determineQuadrant());
293             this.rotateAboutWheel = true;
294             
295             switch(motor){
296                 case MOTOR_A:
297                     //set the rotation velocity
298                     switch(direction){
299                         case FORWARD:
300                             this.setDesiredBearingVelocityXZ(MOTOR_POWER);
301                             break;
302 
303                         case BACKWARD:
304                             this.setDesiredBearingVelocityXZ(-(MOTOR_POWER));
305                             break;
306                     }
307                     break;
308                     
309                 case MOTOR_C:
310                     switch(direction){
311                         case FORWARD:
312                             this.setDesiredBearingVelocityXZ(-(MOTOR_POWER));
313                             break;
314 
315                         case BACKWARD:
316                             this.setDesiredBearingVelocityXZ(MOTOR_POWER);
317                             break;
318                     }
319                     break;
320             }
321                     
322         }
323        
324          /**
325     * Sets a single Motor moving for the given amount of time
326     *
327     * @param motor the motor number (AbstractRobot.MOTOR_A or AbstractRobot.MOTOR_C);
328         * @param direction, the direction number (AbstractRobot.BACKWARDS or AbstractRobot.FORWARDS
329         * @param time the time in milliseconds 
330     */
331         public void singleMotor(int motor, int direction, int time) {
332             this.singleMotor(motor,direction);
333             try{Thread.sleep(time);}catch(Exception e){}
334             stopMoving();
335         } 
336         
337         private int determineQuadrant(){
338             double angle = this.getActualBearingXZ()-this.ANGLE_OFFSET;
339             int motor = this.activeMotor;
340                         
341             //reduce the angle to a value where 0 <= angle <= 360
342             while(angle > 0) angle = angle-360.0;
343             while(angle < 0) angle = angle+360.0;
344                         
345             //angle is 0 <= angle <= 360
346             if(motor == this.MOTOR_C){
347                 angle = angle +180+(2*this.ANGLE_OFFSET);
348                 while(angle > 0) angle = angle-360.0;
349                 while(angle < 0) angle = angle+360.0;
350                 
351                 this.decreasingAngle = angle;
352                 
353             }
354             
355             if(angle >= 0.0 && angle < 90.0) return 1;
356             else if(angle >= 90.0 && angle < 180.0) return 2;
357             else if(angle >= 180.0 && angle < 270.0) return 3;
358             else if(angle >= 270.0 && angle < 360.0) return 4;
359             
360             return 911;
361        
362         }
363         
364         private double getSmallAngle(double angle){
365             
366             while(angle > 0) angle = angle-90.0;
367             while(angle < 0) angle = angle+90.0;
368             
369              //angle is 0 <= angle <= 90
370             return angle;
371         
372         }
373         private void makeQuadrantBasePts(int quadNum){
374             
375             int motor = this.activeMotor;
376             double angle = this.getSmallAngle(this.getActualBearingXZ()-this.ANGLE_OFFSET);
377             
378             if(motor ==  MOTOR_C){
379                 angle = this.getSmallAngle(this.decreasingAngle);
380             }
381                         
382             switch(quadNum){
383                 case 1: 
384 
385                     this.basePointQ1 = new Point2D.Double(this.getXCoord()-this.formula(1,angle),this.getZCoord()+this.formula(0,angle));
386 
387                     this.basePointQ2 = new Point2D.Double(this.basePointQ1.getX()+this.RADIUS,this.basePointQ1.getY()-this.RADIUS);
388                     this.basePointQ3 = new Point2D.Double(this.basePointQ1.getX()+(this.RADIUS*2),this.basePointQ1.getY());
389                     this.basePointQ4 = new Point2D.Double(this.basePointQ1.getX()+this.RADIUS,this.basePointQ1.getY()+this.RADIUS);
390                     break;
391 
392                 case 2:
393                     this.basePointQ2 = new Point2D.Double(this.getXCoord()-this.formula(0,angle),this.getZCoord()-this.formula(1,angle));
394 
395                     this.basePointQ1 = new Point2D.Double(this.basePointQ2.getX()-this.RADIUS,this.basePointQ2.getY()+this.RADIUS);
396                     this.basePointQ3 = new Point2D.Double(this.basePointQ1.getX()+(this.RADIUS*2),this.basePointQ1.getY());
397                     this.basePointQ4 = new Point2D.Double(this.basePointQ1.getX()+this.RADIUS,this.basePointQ1.getY()+this.RADIUS);
398                     break;
399 
400                 case 3:
401                     this.basePointQ3 = new Point2D.Double(this.getXCoord()+this.formula(1,angle),this.getZCoord()-this.formula(0,angle));
402 
403                     this.basePointQ1 = new Point2D.Double(this.basePointQ3.getX()-(this.RADIUS*2),this.basePointQ3.getY());
404                     this.basePointQ2 = new Point2D.Double(this.basePointQ1.getX()+this.RADIUS,this.basePointQ1.getY()-this.RADIUS);
405                     this.basePointQ4 = new Point2D.Double(this.basePointQ1.getX()+this.RADIUS,this.basePointQ1.getY()+this.RADIUS);
406                     break;
407 
408                 case 4:
409                     this.basePointQ4 = new Point2D.Double(this.getXCoord()+this.formula(0,angle),this.getZCoord()+this.formula(1,angle));
410 
411                     this.basePointQ1 = new Point2D.Double(this.basePointQ4.getX()-this.RADIUS,this.basePointQ4.getY()-this.RADIUS);
412                     this.basePointQ2 = new Point2D.Double(this.basePointQ1.getX()+this.RADIUS,this.basePointQ1.getY()-this.RADIUS);
413                     this.basePointQ3 = new Point2D.Double(this.basePointQ1.getX()+(this.RADIUS*2),this.basePointQ1.getY());
414                     break;
415                 
416             }
417                  
418         }
419         
420         private double formula(int a, double angle){
421             
422             angle = (angle/180)*Math.PI;
423             
424             switch(a){
425                 case 0:
426                     return (Math.sin(angle)*this.RADIUS);
427                     
428                 case 1:
429                     return (this.RADIUS*(1-Math.cos(angle)));
430                     
431             }
432             return 911;
433         
434         }
435         
436         public void pivotRotation(){
437             if(this.rotateAboutWheel){
438                 this.moveSimRCXCenter(this.getSmallAngle(this.getActualBearingXZ()-this.ANGLE_OFFSET),this.determineQuadrant());
439             }
440         }
441         
442         private void moveSimRCXCenter(double angle,int quadNum){
443             
444             if(this.activeMotor== this.MOTOR_C){
445                angle = this.getSmallAngle(decreasingAngle);
446             }
447             
448             switch(quadNum){
449                 case 1:
450                     this.setXCoord(this.basePointQ1.getX()+this.formula(1,angle));
451                     this.setZCoord(this.basePointQ1.getY()-this.formula(0, angle));
452                     break;
453 
454                 case 2:
455                     this.setXCoord(this.basePointQ2.getX()+this.formula(0,angle));
456                     this.setZCoord(this.basePointQ2.getY()+this.formula(1, angle));
457                     break;
458 
459                 case 3:
460                     this.setXCoord(this.basePointQ3.getX()-this.formula(1,angle));
461                     this.setZCoord(this.basePointQ3.getY()+this.formula(0, angle));
462                     break;
463                 case 4:
464                     this.setXCoord(this.basePointQ4.getX()-this.formula(0,angle));
465                     this.setZCoord(this.basePointQ4.getY()-this.formula(1, angle));
466                     break;
467                     
468             }
469         }     
470            
471     /**
472     * Stops all motors immediately
473     */
474     public void stopMoving()
475     {
476         this.state=SimRCX.STATE_STOPPED;
477         this.setDesiredVelocity(0);
478         this.setDesiredBearingVelocityXZ(0);
479     }
480     
481     /**
482     * Makes the robot beep
483     */
484     public void beep()
485     {
486         Toolkit.getDefaultToolkit().beep(); 
487                 
488     }
489     
490     /**
491     * Get the current reading of this sensor
492     *
493     * @return the current value
494     */
495     public int getSensor1()
496     {
497         return S1.getValue();
498     }
499     
500     /**
501     * Get the current reading of this sensor
502     *
503     * @return the current value
504     */
505     public int getSensor2()
506     {
507         return S2.getValue();
508     }
509     
510     /**
511     * Get the current reading of this sensor
512     *
513     * @return the current value
514     */
515     public int getSensor3()
516     {
517         return S3.getValue();
518     }
519         
520         /** Clears the display on the LCD
521          *
522          * Modified by: Simon Zienkiewicz
523          *
524          */
525         public void clearLCD() {
526             this.LCD_OUTPUT=" ";
527         }
528         
529         /** Display numbers on the LCD
530          *
531          * Modified by: Simon Zienkiewicz
532          *
533          */
534         public void displayNumberLCD(int num) {
535             this.LCD_OUTPUT=num+"";
536         }
537         
538         /**
539     * Display text on the LCD
540     *
541     * Modified by: Simon Zienkiewicz
542     */
543         public void displayTextLCD(String word) {
544             String actualword;
545             if(word.length() <=5){
546                 actualword = word;
547             }
548             else{
549                 actualword = word.substring(0,5);
550             }
551             
552             this.LCD_OUTPUT=actualword;
553         }
554         public String getLCDOutput(){
555             return this.LCD_OUTPUT;
556         }
557         
558         /** 
559          * Sets the power of the motors for the robot
560          * Modified by: Simon Zienkiewicz
561          *
562          */
563         public void setMotorPower(int power) {
564             /*
565             if(power >= 7){
566                 MOTOR_POWER=8;
567             }
568             else if(power<=0){
569                 MOTOR_POWER=1;
570             }
571             else{
572                 MOTOR_POWER=power;
573             }*/
574             MOTOR_POWER=7;
575         }
576         
577         /**
578     * Gets the power of the motors for the robot
579         * Modified by: Simon Zienkiewicz
580         *
581         * @return the power of the motor 0-7
582     */
583         public int getMotorPower() {
584             return (MOTOR_POWER-1);
585         }
586                 
587         /**
588     * Gets an array of boolean values for the touch sensors
589         * Modified by: Simon Zienkiewicz
590         *
591         * @return boolean values of the status of the sensors
592     */
593         public boolean[] getTouchSensorStatus() {
594             boolean[] touchStatus = new boolean[3];
595             SimSensor[] sensorList = {S1,S2,S3};
596             
597             for(int a=0;a<3;a++){
598                 if(sensors[a]==Controller.SENSOR_TYPE_TOUCH){
599                     touchStatus[a]= ((SimTouchSensor)sensorList[a]).getBooleanValue();
600                 }
601                 else{
602                     touchStatus[a]=false;
603                 }
604             }
605             return touchStatus;
606         }
607         
608         /**
609     * Gets the voltage of the RCX battery in MilliVolts
610         * Modified by: Simon Zienkiewicz
611         *
612         * @return int voltage in the battery of the RCX
613     */
614         public int getVoltage() {
615             return this.BATTERY_VOLTAGE;
616         }        
617                
618         public int createTimer(int time) {
619             josx.util.Timer timer = new josx.util.Timer(time, new TListener(controller,timerList.size()));
620             timerList.addElement(timer);
621             return timerList.indexOf(timer);
622         }
623         
624         public void startTimer(int num) {
625             try{((josx.util.Timer)timerList.elementAt(num)).start();} catch(Exception e){}
626         }
627         
628         public void stopTimer(int num) {
629             try{((josx.util.Timer)timerList.elementAt(num)).stop();} catch(Exception e){}
630         }
631         
632         public void stopAllTimers() {
633             
634             for(int i=0;i<this.timerList.size();i++){
635                 try{((josx.util.Timer)timerList.elementAt(i)).stop();} catch(Exception e){}
636             }
637         }
638         
639         public int getDelay(int num) {
640             try{return((josx.util.Timer)timerList.elementAt(num)).getDelay();} catch(Exception e){}
641             return 911;
642         }
643         
644         public void setDelay(int num, int time) {
645             try{((josx.util.Timer)timerList.elementAt(num)).setDelay(time);} catch(Exception e){}
646         }
647         
648         
649         public void pause(int num) {
650             try{Thread.sleep(num);}
651             catch(Exception e){};
652         }
653         
654         public void activate() {
655             activation = true;
656         }
657         
658         //Event triggers
659         public void lightEvent(int index){
660             if(activation) controller.lightSensorListener(index);
661         }
662         
663         public void touchEvent(int index){
664             if(activation) {
665                 stopMoving();
666                 for(int i=0;i<this.timerList.size();i++){
667                    ((josx.util.Timer)timerList.elementAt(i)).stop();
668                 }
669                 
670                 controller.touchSensorListener(index);        
671             }
672         }
673         
674         public void playTune(int frequency, int duration) {
675             Sound.playTune(frequency, duration);
676         }
677         
678         private class TListener implements TimerListener {
679         /** Reverses the direction of the robot.  Called when the timer times
680         * out.
681         */
682             private int index;
683             private Controller controller;
684             
685             public TListener(Controller c, int num){
686                 controller = c;
687                 index = num;
688             
689             }
690             public void timedOut() {
691                 controller.setTimerExecution(index);
692             }
693         }
694         
695 }
696