public void setSpeed(OutputStream out, byte[] buf, int len) {
int leftSign = 1;
int rightSign = 1;
int p = 2;
if (buf[p] == '-') { // if input is '-', move right one place
leftSign = -1;
p++;
}
int leftPwr = 0;
while ('0' <= buf[p] && buf[p] <= '9')
leftPwr = leftPwr * 10 + (buf[p++] - '0'); // accumulate the decimal value
if (leftPwr > H_MAX_POWER) leftPwr = H_MAX_POWER; // stay in range
leftPwr = leftSign * leftPwr;
// assert(buf[p] == ',');
p++;
if (buf[p] == '-') {
rightSign = -1;
p++;
}
int rightPwr = 0;
while ('0' <= buf[p] && buf[p] <= '9') rightPwr = rightPwr * 10 + (buf[p++] - '0');
if (rightPwr > H_MAX_POWER) rightPwr = H_MAX_POWER; // stay in range
rightPwr = rightSign * rightPwr;
// assert(buf[p] == 0); // the buffer is null-terminated
p++;
leftMotor.setPower((leftPwr * 16) / H_MAX_POWER);
rightMotor.setPower((rightPwr * 16) / H_MAX_POWER);
writeCRLF(out, "d");
}
public void move() {
if (last_move_time == null || last_move_time.doubleValue() < world.time) {
last_move_time = new Double(world.time);
double max_dist, dist_right, dist_left, theta, x, y, dist_diff;
double delta_theta, turn_radius, new_theta, new_x, new_y;
Location location;
Orientation orientation;
orientation = orientation();
location = location();
max_dist = max_speed / world.ticks_per_second;
dist_right = right_motor.output() * max_dist;
dist_left = left_motor.output() * max_dist;
theta = orientation.theta;
x = location.x;
y = location.y;
old_location.x = x;
old_location.y = y;
dist_diff = dist_right - dist_left;
// System.out.println("dist_diff: " + dist_diff);
delta_theta = dist_diff / wheel_base;
if (Math.abs(dist_diff) < .0001) {
turn_radius = 0.0;
} else {
turn_radius = (dist_right / delta_theta) - (wheel_base / 2);
}
// System.out.println("turn_radius: " + turn_radius);
new_theta = theta + delta_theta;
if (turn_radius == 0.0) {
// System.out.println("turn_radius == 0");
new_x = x + Math.cos(theta) * dist_left;
new_y = y + Math.sin(theta) * dist_left;
} else {
// System.out.println("new_theta= " + new_theta + " theta= " + theta);
new_x = x + ((Math.sin(new_theta) - Math.sin(theta)) * turn_radius);
new_y = y - ((Math.cos(new_theta) - Math.cos(theta)) * turn_radius);
}
orientation.theta = new_theta;
location.x = new_x;
location.y = new_y;
maybe_fire_guns();
}
}
// set status led state and wait a second
public static void main(String[] args) throws IOException {
char test = 0;
int servo_position = 6;
System.out.println("NanoVM - console echo demo");
System.out.println("Please press some keys ...");
// adc_value150 이상일경우 흰색 150이하 일경우 검은색
ADC.setEnable(1, ADC.ENABLE);
ADC.setEnable(2, ADC.ENABLE);
ADC.setEnable(3, ADC.ENABLE);
ADC.setEnable(4, ADC.ENABLE);
Servo.release(0);
int adc_value1, adc_value2, adc_value3, adc_value4;
boolean enabled;
while (true) {
if (System.in.available() != 0) {
test = ((char) System.in.read());
System.out.print(test);
} else {
test = 0;
}
if (test == 'w') {
Motor.setSpeed(0, 80);
} else if (test == 's') {
Motor.stop();
}
// Servo.setPosition(0,12);// 12
enabled = ADC.getEnable(1);
adc_value1 = ADC.getADC(1);
System.out.println("adc1 " + adc_value1);
enabled = ADC.getEnable(2);
adc_value2 = ADC.getADC(2);
System.out.println("adc2 " + adc_value2);
enabled = ADC.getEnable(3);
adc_value3 = ADC.getADC(3);
System.out.println("adc3 " + adc_value3);
enabled = ADC.getEnable(4);
adc_value4 = ADC.getADC(4);
System.out.println("adc4 " + adc_value4);
if (adc_value2 < 150 || adc_value3 < 150) {
Servo.setPosition(0, 6);
} else if (adc_value1 < 150 && adc_value2 < 150) {
Servo.setPosition(0, 9);
} else if (adc_value4 < 150 && adc_value3 < 150) {
Servo.setPosition(0, 3);
} else if (adc_value1 < 150) {
Servo.setPosition(0, 9);
} else if (adc_value4 < 150) {
Servo.setPosition(0, 3);
}
Clock.delay_ms(10);
}
}
public void motorsOff(OutputStream out, byte[] buf, int len) {
leftMotor.setPower(0);
rightMotor.setPower(0);
writeCRLF(out, "z");
}