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(); } }