// Vehicle class class Vehicle { float x, y, axle, half_axle, axleSq; float angle; // direction of the Vehicle float wheel_diff, wheel_average; // the difference and average of the wheels' rotating speed myWheel wA, wB; // two wheels Sensor sA, sB; // two sensors int id; Vehicle( float x, float y, float angle, float axle_length, int id ) { this.x = x; this.y = y; this.angle = angle; this.id = id; axle = axle_length; half_axle = axle/2; axleSq = axle*axle; wA = new myWheel( x+half_axle*cos(angle-HALF_PI), y+half_axle*sin(angle-HALF_PI), 10, 0 ); wB = new myWheel( x+half_axle*cos(angle+HALF_PI), y+half_axle*sin(angle+HALF_PI), 10, 0 ); sA = new Sensor( x+axle*cos(angle-HALF_PI/1.5), y+axle*sin(angle-HALF_PI/1.5) ); sB = new Sensor( x+axle*cos(angle+HALF_PI/1.5), y+axle*sin(angle+HALF_PI/1.5) ); } void drawMe() { fill(255,0,0); triangle( wA.x, wA.y, wB.x, wB.y, x+cos(angle)*axle*2.5, y+sin(angle)*axle*2.5 ); //fill(200,54,130); fill(102,102,51); wA.drawMe(angle, 1); wB.drawMe(angle-PI, -1); ellipse( x, y, axle*3, axle*3 ); sA.drawMe(); sB.drawMe(); } // Computes the sensory logic to set the wheel velocity. // Sensor output goes directly to wheel on same side void doSenseLogic() { setASpeed(sA.getSense()); setBSpeed(sB.getSense()); } void move() { checkBounds(); doSenseLogic(); // move wheel_diff = wA.d - wB.d; wheel_average = ( wA.d + wB.d ) / 2; angle += wheel_diff / axle; x += cos( angle ) * wheel_average; y += sin( angle ) * wheel_average; checkCollision(); // wheels move float ang = angle - HALF_PI; wA.x = x + half_axle * cos( ang ); wA.y = y + half_axle * sin( ang ); ang = angle + HALF_PI; wB.x = x + half_axle * cos( ang ); wB.y = y + half_axle * sin( ang ); // sensors move ang = angle - HALF_PI/2; sA.x = x + axle * cos( ang ); sA.y = y + axle * sin( ang ); ang = angle + HALF_PI/2; sB.x = x + axle * cos( ang ); sB.y = y + axle * sin( ang ); } float getX(){ return x; } float getY(){ return y; } void checkBounds() { if (bounded) { x = max( axle+5, min( width-axle-5, x ) ); y = max( axle+5, min( height-axle-5, y ) ); } else { x = ( x<0 ) ? width+x : ((x>width) ? x-width : x ); y = ( y<0 ) ? height+y : ((y>height) ? y-height : y ); } } void checkCollision() { float dx, dy, da; for (int i=0; i returnVal){ returnVal = tempVal; println(sqrt(val1*val1+val2*val2)); } inRange = false; } } return returnVal; } // Returns 1 when sensory value is maximum, 0 when it's minimum //new getSense method to allow vehicles to detect each other float getSense() { boolean inRange = false; float val1 = 0.0; //x value of other robot float val2 = 0.0; //y vlaue of other robot float returnVal = 0; for(int i =0; i returnVal){ //only return the highest value to the vehicle returnVal = tempVal; // println(sqrt(val1*val1+val2*val2)); } inRange = false; } } return returnVal; } void drawMe() { fill(200*sense,0,0); noStroke(); ellipse( x, y, 1, 1 ); } } }