static uint8_t * getSensorData(void){
uint32_t value = getSensorValue();
uint8_t *dataBuffer = malloc(sizeof(uint8_t)*SENSOR_DATA_MAX_LENGTH);
memset(dataBuffer,0,SENSOR_DATA_MAX_LENGTH);
sprintf((char *)dataBuffer,"{\"doorCloseCounter\":\"%lu\"}",value);
return dataBuffer;
}
int main(){
//portInit();
serial_initialize(57600);
dxl_initialize( 0, 1 ); // init with baud = 1 Mbps
ADCInit();
sei(); //Enables global interrupts
unsigned int distanceLeft, distanceRight, front, movingLeft, movingRight;
signed int speedLeft, speedRight;
while(1) {
//printf("%d %d\n\n",getSensorValue(3),getSensorValue(4));
//_delay_ms(1000);
// Get sensor reading (in cm)
front = DMSDistance(getSensorValue(1));
distanceLeft = IRValue(getSensorValue(3));
distanceRight = IRValue(getSensorValue(4));
// Calculating the required speed
speedRight = (int)((front-40*distanceLeft));
speedLeft = (int)((front-40*distanceRight));
movingLeft = dxl_read_byte( 1, 38 );
movingRight = dxl_read_byte(2,38);
if(movingLeft < 15 && movingRight < 15){
wheel(1,0);
wheel(2,-20);
_delay_ms(1000);
}else{
wheel(1,-speedRight);
wheel(2,speedLeft);
}
printf("%d %d - %d\n\n",speedLeft,speedRight,getSensorValue(1));
//_delay_ms(1000);
// Making the wheels spin
}
return 0;
}
int main(int argc, char *argv[])
{
ros::Publisher pub[TOPIC_NR];
int port, plane;
int sock, asock;
struct sigaction act;
memset(&act, 0, sizeof(act));
act.sa_handler = stopChildProcess;
if (sigaction(SIGTERM, &act, NULL) == -1) {
perror("sigaction");
return -1;
}
ros::init(argc, argv, NODE_NAME);
ros::NodeHandle node;
pub[0] = node.advertise<tablet_socket_msgs::gear_cmd>("gear_cmd", 1);
pub[1] = node.advertise<tablet_socket_msgs::mode_cmd>("mode_cmd", 1);
pub[2] = node.advertise<tablet_socket_msgs::route_cmd>("route_cmd", 1);
pub[3] = node.advertise<geometry_msgs::PoseStamped>("gnss_pose", 1);
pub[4] = node.advertise<std_msgs::Bool>("gnss_stat", 1);
node.param<int>("tablet_receiver/port", port, DEFAULT_PORT);
node.param<int>("tablet_receiver/plane", plane, DEFAULT_PLANE);
fprintf(stderr, "listen port=%d\n", port);
geo.set_plane(plane);
//get connect to android
sock = -1;
sigaction(SIGINT, NULL, &act);
act.sa_flags &= ~SA_RESTART;
sigaction(SIGINT, &act, NULL);
while (getConnect(port, &sock, &asock) != -1) {
struct timeval tv[2];
double sec;
int count;
fprintf(stderr, "get connect.\n");
gettimeofday(tv, NULL);
for (count = 0; ; count++) {
if(getSensorValue(asock, pub) == -1)
break;
if(sendSignal(asock) == -1)
break;
}
close(asock);
gettimeofday(tv+1, NULL);
sec = (tv[1].tv_sec - tv[0].tv_sec) +
(tv[1].tv_usec - tv[0].tv_usec) / 1000000.0;
fprintf(stderr, "done, %f sec\n",sec);
}
return 0;
}
/*
* Makes the robot turn on the spot.
* @param state The state to move into, should be in the interval [1;4]
* @param angle Determines how long steps to take
* @return The distance in cm read from the DMS sensor
*/
int turn(int state,int angle){
switch(state){
case 1:
dxl_write_word(FRONT_LEFT_ROTATE, GOAL_POSITION_L, convert(angle));
dxl_write_word(FRONT_RIGHT_ROTATE, GOAL_POSITION_L, convert(angle));
dxl_write_word(BACK_LEFT_ROTATE, GOAL_POSITION_L, convert(angle));
dxl_write_word(BACK_RIGHT_ROTATE, GOAL_POSITION_L, convert(angle));
_delay_ms(200);
dxl_write_word(FRONT_LEFT_BEND, GOAL_POSITION_L, convert(-40));
_delay_ms(200);
dxl_write_word(FRONT_LEFT_ROTATE, GOAL_POSITION_L, convert(-angle));
_delay_ms(200);
dxl_write_word(FRONT_LEFT_BEND, GOAL_POSITION_L, convert(0));
//_delay_ms(200);
break;
case 2:
dxl_write_word(FRONT_RIGHT_BEND, GOAL_POSITION_L, convert(40));
_delay_ms(200);
dxl_write_word(FRONT_RIGHT_ROTATE, GOAL_POSITION_L, convert(-angle));
_delay_ms(200);
dxl_write_word(FRONT_RIGHT_BEND, GOAL_POSITION_L, convert(0));
//_delay_ms(200);
break;
case 3:
dxl_write_word(BACK_LEFT_BEND, GOAL_POSITION_L, convert(40));
_delay_ms(200);
dxl_write_word(BACK_LEFT_ROTATE, GOAL_POSITION_L, convert(-angle));
_delay_ms(200);
dxl_write_word(BACK_LEFT_BEND, GOAL_POSITION_L, convert(0));
//_delay_ms(200);
break;
case 4:
dxl_write_word(BACK_RIGHT_BEND, GOAL_POSITION_L, convert(-40));
_delay_ms(200);
dxl_write_word(BACK_RIGHT_ROTATE, GOAL_POSITION_L, convert(-angle));
_delay_ms(200);
dxl_write_word(BACK_RIGHT_BEND, GOAL_POSITION_L, convert(0));
//_delay_ms(200);
break;
}
_delay_ms(200);
return DMSDistance(getSensorValue(6));
}
IOReturn FakeSMCPlugin::callPlatformFunction(const OSSymbol *functionName, bool waitForFunction, void *param1, void *param2, void *param3, void *param4 )
{
if (functionName->isEqualTo(kFakeSMCGetValueCallback)) {
const char *name = (const char*)param1;
void *data = param2;
UInt8 size = (UInt64)param3;
if (name && data)
if (FakeSMCSensor *sensor = getSensor(name))
if (size == sensor->getSize()) {
sensor->encodeValue(getSensorValue(sensor), data);
return kIOReturnSuccess;
}
return kIOReturnBadArgument;
}
return super::callPlatformFunction(functionName, waitForFunction, param1, param2, param3, param4);
}
float getTemperature()
{
int32_t ADC_reading = 0;
// float Vcc = 3.3;
// float Ve = 2.048;
ADC_reading = getSensorValue();
// DEBUG("ADC Reading: ") DEBUGln(ADC_reading);
// float therm_res = (SERIESRESISTOR * Vcc * ADC_reading)/((16383.0*Ve)-(Vcc*ADC_reading));
// float therm_res = (SERIESRESISTOR * Ve * ADC_reading)/((16383.0*Ve)-(Ve*ADC_reading));
float therm_res = 10000.0/(32767.0/ADC_reading-1.0);
// DEBUG("Thermistor Voltage = "); DEBUGln(ADC_reading*Ve/32767);
// DEBUG("Thermistor Resistance = ") DEBUGln(therm_res);
float steinhart;
steinhart = therm_res / THERMISTORNOMINAL; // (R/Ro)
steinhart = log(steinhart); // ln(R/Ro)
steinhart /= BCOEFFICIENT; // 1/B * ln(R/Ro)
steinhart += 1.0 / (TEMPERATURENOMINAL + 273.15); // + (1/To)
steinhart = 1.0 / steinhart; // Invert
steinhart -= 273.15; // convert to C
// DEBUG("Temperature "); DEBUG(steinhart); DEBUGln(" *C");
return steinhart;
}
void readSensors() {
tempRoom = getSensorValue(SENSOR_TEMP);
humRoom = getSensorValue(SENSOR_HUM);
}
bool SetMovement(uint8_t key[])
{
// Set Movement Flags
bool down = (key[1] == uint8_t('d'));
bool down_only = false;
// only down keys
if(down)
{
switch(char(key[0]))
{
//speed (1 2 3 on keyboard Oo)
case 'a': speed = 50; break;
case 'b': speed = 100; break;
case 'c': speed = 127; break;
case 'R': // reset encoders
re.clear();
le.clear();
break;
case 'Q': // on/off correction
rs232.send("Engine correction is ");
if(state & STATE_CORRECTION)
{
state &= ~(STATE_CORRECTION);
rs232.send("disabled \r\n");
}
else
{
state |= STATE_CORRECTION;
rs232.send("enabled \r\n");
}
break;
case 'C':
rs232.wait();
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
if(!(state & STATE_RECORD))
{
state |= STATE_RECORD;
recordIter = 0;
rs232.send("Erasing EEPROM...");
state |= STATE_ERASING;
erase_eeprom();
state &= ~(STATE_ERASING);
rs232.send("done\r\n");
lastAdress = 0;
}
else
{
lastRec.end_event = EVENT_TIME;
lastRec.setBigNum(recordTime.getTime()/10000);
write_mem(&lastRec, lastAdress);
recordTime.stop();
recordTime.clear();
recordIter = 0;
state &= ~(STATE_RECORD);
}
rs232.send("Trace recording is ");
if(state & STATE_RECORD){ rs232.send("enabled \r\n");}
else {rs232.send("disabled \r\n");}
break;
case 'P':
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
setMotorPower(0, 0);
if(!(state & STATE_PLAY))
{
recordTime.stop();
recordTime.clear();
rs232.send("Playing..\r\n");
recordIter = 0;
lastAdress = 0;
state |= STATE_PLAY;
state &= ~(STATE_RECORD);
}
else
{
rs232.send("Playback stopped\r\n");
state &= ~(STATE_PLAY);
}
break;
case 'O':
if(state & STATE_RECORD)
break;
rs232.send("Playback ");
if(state & STATE_PLAY)
{
rs232.send("unpaused\r\n");
state &= ~(STATE_PLAY);
}
else
{
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
moveflags = MOVE_NONE;
rs232.send("paused\r\n");
state |= STATE_PLAY;
}
break;
}
}
// Movement
switch(char(key[0]))
{
case 'W':
if(!(moveflags & MOVE_BACKWARD))
{
if(down) moveflags |= MOVE_FORWARD;
else moveflags &= ~(MOVE_FORWARD);
}
break;
case 'S':
if(!(moveflags & MOVE_FORWARD))
{
if(down) moveflags |= MOVE_BACKWARD;
else moveflags &= ~(MOVE_BACKWARD);
}
break;
case 'A':
if(!(moveflags & MOVE_RIGHT))
{
if(down) moveflags |= MOVE_LEFT;
else moveflags &= ~(MOVE_LEFT);
}
break;
case 'D':
if(!(moveflags & MOVE_LEFT))
{
if(down) moveflags |= MOVE_RIGHT;
else moveflags &= ~(MOVE_RIGHT);
}
break;
default:
down_only = true;
break;
}
// Sensors
if(char(key[0]) == ' ' && down) // Space
{
rs232.wait();
rs232.send("\r\nSensors: ");
rs232.dumpNumber(getSensorValue(6));
rs232.sendNumber(getSensorValue(7)); // proud
/*rs232.send("\r\nSensors: \r\n");
rs232.send(" ");
rs232.sendNumber(getSensorValue(5));
rs232.send(" ");
rs232.sendNumber(getSensorValue(1));
rs232.wait();
rs232.send("\r\n");
rs232.sendNumber(getSensorValue(2));
rs232.send(" ");
rs232.sendNumber(getSensorValue(3));
rs232.wait(); */
rs232.send("\r\nEncoders: \r\n L: ");
rs232.sendNumber(le.get());
rs232.send(" R: ");
rs232.sendNumber(re.get());
rs232.send("\r\nRange \r\nL: ");
rs232.wait();
rs232.sendNumber(ReadRange(FINDER_LEFT));
rs232.send("cm M: ");
rs232.sendNumber(ReadRange(FINDER_MIDDLE));
rs232.send("cm R: ");
rs232.sendNumber(ReadRange(FINDER_RIGHT));
rs232.send("cm\r\n");
}
//Set motors
if(moveflags & MOVE_FORWARD)
{
if(moveflags & MOVE_LEFT)
setMotorPower(speed-TURN_VALUE, speed);
else if(moveflags & MOVE_RIGHT)
setMotorPower(speed, speed-TURN_VALUE);
else
{
le_cor.start();
re_cor.start();
le_cor.clear();
re_cor.clear();
setMotorPower(speed, speed);
state &= ~(STATE_CORRECTION2);
}
startTime = getTickCount();
}
else if(moveflags & MOVE_BACKWARD)
{
if(moveflags & MOVE_LEFT)
setMotorPower(-(speed-TURN_VALUE), -speed);
else if(moveflags & MOVE_RIGHT)
setMotorPower(-speed, -(speed-TURN_VALUE));
else
{
state &= ~(STATE_CORRECTION2);
le_cor.start();
re_cor.start();
le_cor.clear();
re_cor.clear();
setMotorPower(-speed, -speed);
}
startTime = getTickCount();
}
else if(moveflags & MOVE_LEFT)
{
setMotorPower(-speed, speed);
startTime = getTickCount();
}
else if(moveflags & MOVE_RIGHT)
{
setMotorPower(speed, -speed);
startTime = getTickCount();
}
else
{
startTime = getTickCount();
setMotorPower(0, 0);
le_cor.stop();
re_cor.stop();
state &= ~(STATE_CORRECTION2);
}
return down_only;
}
void loop(void) {
loopCount++;
if (digitalRead(BTN_PIN) == LOW) {
leftCounter = 0;
rightCounter = 0;
delay(1000);
driveWheel(WHEEL_LEFT, 150);
driveWheel(WHEEL_RIGHT, 150);
printDelay = millis();
}
tmpTime = micros();
if ((leftReadedTime + ANALOG_READ_DELAY < tmpTime)
|| (leftReadedTime > tmpTime)
) {
tmpValue = analogRead(LEFT_WHEEL_ENCODER_PIN);
tmpIndex = leftReadedCount % ANALOG_READ_BUFFER_COUNT;
leftReaded[tmpIndex] = tmpValue;
tmpValue += leftReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 1) % ANALOG_READ_BUFFER_COUNT];
tmpValue += leftReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 2) % ANALOG_READ_BUFFER_COUNT];
if (tmpValue <= LOW_VALUE_THRESHOLD) {
if (leftEncoderState) {
leftEncoderState = 0;
}
} else if (tmpValue >= HIGH_VALUE_THRESHOLD) {
if (!leftEncoderState) {
leftEncoderState = 1;
leftCounter++;
}
}
leftReadedCount++;
leftReadedTime = tmpTime;
}
tmpTime = micros();
if ((rightReadedTime + ANALOG_READ_DELAY < tmpTime)
|| (rightReadedTime > tmpTime)
) {
tmpValue = analogRead(RIGHT_WHEEL_ENCODER_PIN);
tmpIndex = rightReadedCount % ANALOG_READ_BUFFER_COUNT;
rightReaded[tmpIndex] = tmpValue;
tmpValue += rightReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 1) % ANALOG_READ_BUFFER_COUNT];
tmpValue += rightReaded[(ANALOG_READ_BUFFER_COUNT + tmpIndex - 2) % ANALOG_READ_BUFFER_COUNT];
if (tmpValue <= LOW_VALUE_THRESHOLD) {
if (rightEncoderState) {
rightEncoderState = 0;
}
} else if (tmpValue >= HIGH_VALUE_THRESHOLD) {
if (!rightEncoderState) {
rightEncoderState = 1;
rightCounter++;
}
}
rightReadedCount++;
rightReadedTime = tmpTime;
}
tmpTime = micros();
if ((sensorReadedTime + SENSOR_READ_DELAY < tmpTime)
|| (leftReadedTime > tmpTime)
) {
tmpIndex = (sensorReadedCount % SENSOR_READ_BUFFER_COUNT);
for (int i = 0; i < LASER_SENSORS_COUNT; i++) {
analogSensorValue[i][tmpIndex] = analogRead(laserSensors[i]);
}
sensorReadedCount++;
sensorReadedTime = tmpTime;
}
if (printDelay + 1000 < millis()) {
stopWheel(WHEEL_LEFT);
stopWheel(WHEEL_RIGHT);
Serial.print("left=");
Serial.print(leftCounter);
Serial.print(" right=");
Serial.print(rightCounter);
Serial.print(" loops=");
Serial.print(loopCount);
for (int i = 0; i < LASER_SENSORS_COUNT; i++) {
Serial.print("\tS");
Serial.print(i);
Serial.print("=");
Serial.print(getSensorValue(i));
}
Serial.println();
loopCount = 0;
printDelay = millis();
}
}
int Controller::getIRRightValue()
{
return getSensorValue(1);
}
int Controller::getIRLeftValue()
{
return getSensorValue(0);
}
int Controller::getWhiskerRightValue()
{
return getSensorValue(7);
}
int Controller::getWhiskerLeftValue()
{
return getSensorValue(6);
}
/*
* Makes the robot move into a given state.
* @param state The state to move into, should be in the interval [1;4]
* @param left_angle Determines how much the left side of the robot need to move
* @param right_angle Determines how much the right side of the robot need to move
* @return The distance in cm read from the DMS sensor
*/
int forward(int state,int left_angle, int right_angle){
int head_angle = 60;
switch(state){
case 1:
dxl_write_word(FRONT_LEFT_ROTATE, GOAL_POSITION_L, convert(left_angle));
dxl_write_word(FRONT_RIGHT_ROTATE, GOAL_POSITION_L, convert(right_angle));
dxl_write_word(BACK_LEFT_ROTATE, GOAL_POSITION_L, convert(-left_angle));
dxl_write_word(BACK_RIGHT_ROTATE, GOAL_POSITION_L, convert(-right_angle));
dxl_write_word(FRONT_LEFT_BEND, GOAL_POSITION_L, convert(0));
dxl_write_word(FRONT_RIGHT_BEND, GOAL_POSITION_L, convert(40));
dxl_write_word(BACK_LEFT_BEND, GOAL_POSITION_L, convert(40));
dxl_write_word(BACK_RIGHT_BEND, GOAL_POSITION_L, convert(0));
dxl_write_word(HEAD, GOAL_POSITION_L, convert(-head_angle));
break;
case 2:
dxl_write_word(FRONT_LEFT_ROTATE, GOAL_POSITION_L, convert(-left_angle));
dxl_write_word(FRONT_RIGHT_ROTATE, GOAL_POSITION_L, convert(-right_angle));
dxl_write_word(BACK_LEFT_ROTATE, GOAL_POSITION_L, convert(left_angle));
dxl_write_word(BACK_RIGHT_ROTATE, GOAL_POSITION_L, convert(right_angle));
dxl_write_word(FRONT_LEFT_BEND, GOAL_POSITION_L, convert(0));
dxl_write_word(FRONT_RIGHT_BEND, GOAL_POSITION_L, convert(40));
dxl_write_word(BACK_LEFT_BEND, GOAL_POSITION_L, convert(40));
dxl_write_word(BACK_RIGHT_BEND, GOAL_POSITION_L, convert(0));
dxl_write_word(HEAD, GOAL_POSITION_L, convert(0));
break;
case 3:
dxl_write_word(FRONT_LEFT_ROTATE, GOAL_POSITION_L, convert(-left_angle));
dxl_write_word(FRONT_RIGHT_ROTATE, GOAL_POSITION_L, convert(-right_angle));
dxl_write_word(BACK_LEFT_ROTATE, GOAL_POSITION_L, convert(left_angle));
dxl_write_word(BACK_RIGHT_ROTATE, GOAL_POSITION_L, convert(right_angle));
dxl_write_word(FRONT_LEFT_BEND, GOAL_POSITION_L, convert(-40));
dxl_write_word(FRONT_RIGHT_BEND, GOAL_POSITION_L, convert(0));
dxl_write_word(BACK_LEFT_BEND, GOAL_POSITION_L, convert(0));
dxl_write_word(BACK_RIGHT_BEND, GOAL_POSITION_L, convert(-40));
dxl_write_word(HEAD, GOAL_POSITION_L, convert(head_angle));
break;
case 4:
dxl_write_word(FRONT_LEFT_ROTATE, GOAL_POSITION_L, convert(left_angle));
dxl_write_word(FRONT_RIGHT_ROTATE, GOAL_POSITION_L, convert(right_angle));
dxl_write_word(BACK_LEFT_ROTATE, GOAL_POSITION_L, convert(-left_angle));
dxl_write_word(BACK_RIGHT_ROTATE, GOAL_POSITION_L, convert(-right_angle));
dxl_write_word(FRONT_LEFT_BEND, GOAL_POSITION_L, convert(-40));
dxl_write_word(FRONT_RIGHT_BEND, GOAL_POSITION_L, convert(0));
dxl_write_word(BACK_LEFT_BEND, GOAL_POSITION_L, convert(0));
dxl_write_word(BACK_RIGHT_BEND, GOAL_POSITION_L, convert(-40));
dxl_write_word(HEAD, GOAL_POSITION_L, convert(0));
break;
}
_delay_ms(160);
//sprintf("%d\n",getSensorValue(6))
return DMSDistance(getSensorValue(6));
}
//The main invoker routine. It takes as argument the next command to execute and does what is necessary
//Self-explanatory code!
void my_invoker (unsigned char command) {
if(command == BUZZER_ON){
buzzer_on();
return;
}
else if(command == BUZZER_OFF){
buzzer_off();
return;
}
else if(command == MOVE_FORWARD)
{
forward(); //forward
return;
}
else if(command == MOVE_BACKWARD)
{
back(); //back
return;
}
else if(command == MOVE_LEFT)
{
left(); //left
return;
}
else if(command == MOVE_RIGHT)
{
right(); //right
return;
}
else if(command == STOP)
{
stop(); //stop
return;
}
else if(command == SET_VELOCITY)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
//assert(numargs == 1);
int velleft = (int)*(ch);
int velright = (int)*(ch+1);
velocity(velleft,velright);
return;
}
else if(command == MOVE_BY)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
int pos_a = (int)*(ch);
int pos_b = (int)*(ch+1);
//int pos = 10;
//while (pos_b--) pos *= 10;
//pos *= pos_a;
//forward_mm(pos);
pos_a += (pos_b << 8);
forward();
velocity(120,120);
while (pos_a--) {
//delay on 5 ms
stop_on_timer4_overflow = 1;
start_timer4();
while (stop_on_timer4_overflow != 0) {;}
}
stop();
send_char(SUCCESS);
leftInt = 0;
rightInt = 0;
return;
}
else if(command == MOVE_BACK_BY)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
int pos_a = (int)*(ch);
int pos_b = (int)*(ch+1);
//int pos = 10;
//while (pos_b--) pos *= 10;
//pos *= pos_a;
//forward_mm(pos);
pos_a += (pos_b << 8);
back();
velocity(120,120);
while (pos_a--) {
//delay on 5 ms
stop_on_timer4_overflow = 1;
start_timer4();
while (stop_on_timer4_overflow != 0) {;}
}
stop();
send_char(SUCCESS);
leftInt = 0;
rightInt = 0;
return;
}
else if(command == TURN_LEFT_BY)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
already_stopped = 0;
int pos_a = (int)*(ch);
int pos_b = (int)*(ch+1);
pos_a += (pos_b << 8);
_delay_ms(500);
left();
velocity(200,200);
while (pos_a--) {
//delay on 5 ms
stop_on_timer4_overflow = 1;
start_timer4();
while (stop_on_timer4_overflow != 0) {;}
}
stop();
send_char(SUCCESS);
leftInt = 0;
rightInt = 0;
already_modified_stopped = 0;
return;
}
else if(command == TURN_RIGHT_BY)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
//assert(numargs == 2);
int pos_a = (int)*(ch);
int pos_b = (int)*(ch+1);
pos_a += (pos_b << 8);
_delay_ms(500);
right();
velocity(200,200);
while (pos_a--) {
//delay on 5 ms
stop_on_timer4_overflow = 1;
start_timer4();
while (stop_on_timer4_overflow != 0) {;}
}
stop();
send_char(SUCCESS);
leftInt = 0;
rightInt = 0;
already_modified_stopped = 0;
return;
}
else if(command == LCD_SET_STRING)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
int i =0;
lcd_clear();
for(;i<numargs;i++)
{
lcd_wr_char(*(ch+i));
}
return;
}
else if (command == SET_PORT){
int numargs;
unsigned char * ch = recieve_args(&numargs); ;
if (numargs != 2){
}
int portnum = (int) *(ch);
unsigned char value = (unsigned char) *(ch+1);
setPort(portnum,value);
}
else if(command == GET_SENSOR_VALUE)
{
int numargs;
unsigned char * ch = recieve_args(&numargs); ;
if (numargs != 1){
}
int sensornum = (int) *(ch);
//setPort(portnum,value);
getSensorValue(sensornum);
}
else if(command == GET_PORT)
{
int numargs;
unsigned char * ch = recieve_args(&numargs); ;
if (numargs != 1){
}
int portnum = (int) *(ch);
getPort(portnum);
}
else if (command == WHITELINE_FOLLOW_START) {
whiteline_follow_start();
}
else if(command == PRINT_STATE){
buzzer_on();
lcd_num(state);
_delay_ms(1000);
buzzer_off();
}
else if (command == WHITELINE_FOLLOW_END) {
whiteline_follow_end();
}
else if (command == WHITELINE_STOP_INTERSECTION) {
whiteline_stop_intersection_flag = 1;
}
else if(command == ACC_START) {
acc_flag = 1;
}
else if(command == ACC_STOP) {
acc_flag = 0;
acc_modified_flag = 0;
buzzer_off();
}
else if(command == ACC_MODIFIED){
acc_modified_flag = 1;
already_modified_stopped = 0;
}
else if(command == ACC_CHECK){
if (acc_modified_flag == 1 && already_modified_stopped == 1){
send_char((char)1);
}
else {
char value = PORTA;
if (value == 0) send_char((char)2);
else send_char((char)0);
}
}
else if (command == ENABLE_LEFT_WHEEL_INTERRUPT) {
leftInt = 0;
left_position_encoder_interrupt_init();
}
else if (command == ENABLE_RIGHT_WHEEL_INTERRUPT) {
rightInt = 0;
right_position_encoder_interrupt_init();
}
else if (command == GET_LEFT_WHEEL_INTERRUPT_COUNT) {
send_int (leftInt);
leftInt = 0;
}
else if (command == GET_RIGHT_WHEEL_INTERRUPT_COUNT) {
send_int (rightInt);
rightInt = 0;
}
else if (command == SET_TIMER) {
int numargs;
unsigned char * ch = recieve_args(&numargs); ;
if (numargs != 1){
}
int time = (int) *(ch);
timer4_init2(time);
}
else if (command == DISCONNECT) {
disconnect();
}
else { //Error!!! Unrecognized Command
buzzer_on();
_delay_ms(1000);
buzzer_off();
}
}