void adjustSpeed(short leftEnc, short rightEnc, int targetSpeed, uint8_t firstCall){ #define getSpeed(old,del) (((old)>64)?((old) + (delta)):( ((old)==64)?(64):((old)-(delta)) )) //Increase magnitude of old if not 64 static int oldDelta = 0; if( firstCall ) oldDelta = 0; //Reset Delta //Get speed int curSpeed = enc2Speed(leftEnc,rightEnc); int diff = curSpeed - targetSpeed; int delta = 0; uint8_t left, right; int newLeft, newRight; //Calculate delta if( diff > 0 || diff < -1 ){ //If outside of tolerance of -1 to 0 //Speed Up delta = diff*diff * SPD_DELTA; if( delta > MAX_SPD_DELTA ) delta = MAX_SPD_DELTA; //Slow Down if( targetSpeed < curSpeed ) delta = -delta; //printw("delta = %d\ttarget = %d\t current = %f\n",delta,targetSpeed,curSpeed); } //else do nothing; //Get motor values getMotorData(&motorData, &left, &right); newLeft = getSpeed(left,oldDelat+delta); newRight = getSpeed(right,oldDelta+delta); //Contrain values if( newLeft > 127 || newLeft < 1 || \ newRight > 127 || newLeft < 1){ //Delta is too large, adjust it if( delta > 0 ){ uint8_t maxMotor = max( newLeft, newRight); delta -= maxMotor-127; } else{ uint8_t minMotor = min( newLeft, newRight ); delta += 1-minMotor; } //Set new motor values newLeft = getSpeed(left,oldDelta+delta); newRight = getSpeed(right,oldDelta+delta); } oldDelta += delta; //apply motor values setMotorData(&motorData, newLeft, newRight); }
// This is the actual task that is run static portTASK_FUNCTION( navigationUpdateTask, pvParameters ) { // Get the parameters navStruct *navData = (navStruct *) pvParameters; // Buffer for receiving messages g9Msg msgBuffer; uint8_t numLap=0; uint8_t lineDist=0; uint16_t curHeading=0; curState = straight; portTickType ticksAtStart=0; //in ticks portTickType tagTime=0; //Distance where tag was found // Like all good tasks, this should never exit for(;;) { int16_t enc[2] = {0,0}; //Encoder value // Wait for a message from the "otherside" portBASE_TYPE ret; if ((ret = xQueueReceive(navData->inQ,(void *) &msgBuffer,1500) ) != pdTRUE) { //VT_HANDLE_FATAL_ERROR(ret); printw_err("NavTask Didn't RCV From PIC!\n"); setMotorData(&motorData,64,64); setState("Stopped"); goto end; //Skip everything send stop } g9Msg msg; // Now, based on the type of the message, we decide on the action to take switch (msgBuffer.msgType){ case navLineFoundMsg: //stop we have found the finish line!!! lineFound = TRUE; lineDist = totDist; break; case navIRDataMsg: //Save the data LEFT_FRONT_IR = ir2Dist(msgBuffer.buf[0],LEFT_FRONT_IR); RIGHT_FRONT_IR = ir2Dist(msgBuffer.buf[1],RIGHT_FRONT_IR); LEFT_BACK_IR = ir2Dist(msgBuffer.buf[2],LEFT_BACK_IR); RIGHT_BACK_IR = ir2Dist(msgBuffer.buf[3],RIGHT_BACK_IR); SONAR_LEFT = ir2Dist(msgBuffer.buf[4],SONAR_LEFT); SONAR_RIGHT = ir2Dist(msgBuffer.buf[5],SONAR_RIGHT); oldIR[0][irIndex] = LEFT_FRONT_IR; oldIR[1][irIndex] = RIGHT_FRONT_IR; oldIR[2][irIndex] = LEFT_BACK_IR; oldIR[3][irIndex] = RIGHT_BACK_IR; oldIR[4][irIndex] = SONAR_LEFT; oldIR[5][irIndex] = SONAR_RIGHT; irIndex = (irIndex+1)%5; /* LEFT_FRONT_IR = avgIr(oldIR[0]); RIGHT_FRONT_IR = avgIr(oldIR[1]); LEFT_BACK_IR = avgIr(oldIR[2]); RIGHT_BACK_IR = avgIr(oldIR[3]); SONAR_LEFT = avgIr(oldIR[4]); SONAR_RIGHT = avgIr(oldIR[5]);*/ msg.msgType = webIRMsg; msg.id = 0; //internal msg.length = 6*sizeof(uint8_t); memcpy(msg.buf,IR,msg.length); SendConductorMsg(&msg,10); msg.msgType = webPowerMsg; msg.id = 0; //internal msg.length = sizeof(uint16_t); ((uint16_t*)msg.buf)[0] = (uint16_t)(msgBuffer.buf[6])*2; SendConductorMsg(&msg,10); break; case navEncoderMsg: { static portTickType oldTick=0; portTickType newTick = xTaskGetTickCount(); //Get encoder values enc[0] = ((short*)msgBuffer.buf)[0]; enc[1] = ((short*)msgBuffer.buf)[1]; totDist += enc2Dist((enc[0]+enc[1])/2); //get current heading uint16_t tAngle = enc2Ang(enc[0], enc[1]); if(tAngle<0) tAngle = 360 - tAngle; curHeading += tAngle; if(curHeading>359) curHeading -= 360; //calculate poll rate enc_poll_rate = ((newTick-oldTick)*portTICK_RATE_MS); oldEnc[0] = enc[0]; oldEnc[1] = enc[1]; oldTick=newTick; break; } case navRFIDFoundMsg: //Save the data and make a decision if( (xTaskGetTickCount() - tagTime)*portTICK_RATE_MS > 3000 ){ if( !(tagValue & Finish) && (msgBuffer.buf[0] == Finish) ){ if( (numLap++ == 0) && inputs.loop==1 ){ disableTag(Finish); } } printw_err("Handle tag: 0x%X\n",msgBuffer.buf[0]); tagTime = xTaskGetTickCount(); //Found tag record where tagValue |= msgBuffer.buf[0]; //Store tag info } break; case navWebInputMsg: { //printw("Inputs are HERE!\n"); uint8_t oldStart = inputs.start; inputs = ((webInput_t*)msgBuffer.buf)[0]; if( oldStart == 0 && inputs.start == 1 ){ printw("<b style=color:green>Starting Navigation</b>\n"); //Get start time (in ticks) ticksAtStart = xTaskGetTickCount(); //Reset Distance totDist = 0; //Reset RFID tags tagValue = 0; } break; } default: //Invalid message type - Should have been handled in conductor VT_HANDLE_FATAL_ERROR(INVALID_G9MSG_TYPE); break; } curMemLoc = (totDist/TRACK_MEM_DIST) % TRACK_MEM_SIZE; trackMem[curMemLoc]->left = LEFT_FRONT_IR; trackMem[curMemLoc]->right = RIGHT_FRONT_IR; trackMem[curMemLoc]->heading = curHeading; if(inputs.start==1){ setState("Navigate"); stateMachine(); handleSpecialEvents(enc); //Do additional slow down if line was found if( lineFound ){ int speed = enc2Speed(oldEnc[0],oldEnc[1]); if( speed > 10 ) speed -= 8; if( speed < -10 ) speed += 8; adjustSpeed(oldEnc[0],oldEnc[1],speed,0); } //if we can't find the line in 20 cm, lets stop looking if(lineDist <totDist +20) lineFound = FALSE; } else{ setMotorData(&motorData,64,64); setState("Stopped"); } end: msg.msgType = navMotorCmdMsg; msg.id = msgBuffer.id + 1; msg.length = 2; msg.buf[0] = motorData.left; msg.buf[1] = motorData.right; vtLEDOn(0x01); SendZigBeeMsg(navData->zigBeePtr,&msg,portMAX_DELAY); vtLEDOff(0x01); //Send off web information msg.msgType = webMotorMsg; msg.id = 0; //internal msg.length = 2; getMotorData(&motorData,&msg.buf[0],&msg.buf[1]); SendConductorMsg(&msg,10); msg.msgType = webSpeedMsg; msg.id = 0; //internal msg.length = 2*sizeof(int); ((int*)msg.buf)[0] = (int)enc2Speed(oldEnc[0],oldEnc[1]); ((int*)msg.buf)[1] = (1000*totDist)/((xTaskGetTickCount()-ticksAtStart)*portTICK_RATE_MS); // cm/(ticks*(ms/ticks)) = cm/ms * 1000 ms/s = cm/s SendConductorMsg(&msg,10); msg.msgType = webStateMsg; msg.id = 0; //internal msg.length = strlen(state); strcpy((char*)msg.buf,state); SendConductorMsg(&msg,10); if( ~(tagValue & Finish) && (inputs.start == 1)){ msg.msgType = webTimeMsg; msg.id = 0; //internal msg.length = 2*sizeof(unsigned int); ((unsigned int*)msg.buf)[1] = (xTaskGetTickCount()-ticksAtStart)*portTICK_RATE_MS; //actual ((unsigned int*)msg.buf)[0] = (((unsigned int*)msg.buf)[1]*33)/((1000*totDist)/((unsigned int*)msg.buf)[1]); // nominal = (actual)* 33/(avg Speed) SendConductorMsg(&msg,10); } msg.msgType = webNavMsg; msg.id = 0; //internal msg.length = 3*sizeof(uint8_t) + 2*sizeof(int16_t); msg.buf[0] = (tagValue & Finish) > 0; msg.buf[1] = numLap; msg.buf[2] = tagValue; memcpy(&(msg.buf[3]),oldEnc,2*sizeof(int16_t)); SendConductorMsg(&msg,10); } }
void neural_task(void *p) { while(1){ detachInterrupt(EXTI_Line0); /*close external interrupt 0*/ detachInterrupt(EXTI_Line1); /*close external interrupt 1*/ detachInterrupt(EXTI_Line2); /*close external interrupt 2*/ detachInterrupt(EXTI_Line3); /*close external interrupt 3*/ if(car_state == CAR_STATE_MOVE_FORWARD){ getMotorData(); rin = move_speed; neural_update(&n_r, rin, speed_right_counter_1, distance_right_counter); neural_update(&n_l, rin, speed_left_counter_1, distance_left_counter); data_record(); float input_l = n_l.u; float input_r = n_r.u; proc_cmd("forward", base_pwm_l+(int)input_l, base_pwm_r+(int)input_r); } else if (car_state == CAR_STATE_MOVE_BACK) { getMotorData(); float err = 0; rin = 10; neural_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter); neural_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter); float input_l = n_l_back.u_1; float input_r = n_r_back.u_1; proc_cmd("forward", base_pwm_l - input_l, base_pwm_r - input_r); } else if (car_state == CAR_STATE_MOVE_LEFT){ rin = 5; getMotorData(); if (distance_right_counter > MOVE_LEFT_RIGHT_PERIOD) { rin = 0; pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter); }else{ pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter); } if (distance_left_counter > MOVE_LEFT_RIGHT_PERIOD) { rin = 0; pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter); }else{ pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter); } float input_l = n_l_back.u; float input_r = n_r.u; proc_cmd("left", base_pwm_l-(int)input_l, base_pwm_r+(int)input_r); } else if (car_state == CAR_STATE_MOVE_RIGHT){ rin = 5; getMotorData(); if (distance_right_counter > MOVE_LEFT_RIGHT_PERIOD) { rin = 0; pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter); }else{ pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter); } if (distance_left_counter > MOVE_LEFT_RIGHT_PERIOD) { rin = 0; pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter); }else{ pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter); } float input_l = n_l.u; float input_r = n_r_back.u; proc_cmd("right", base_pwm_l+(int)input_l, base_pwm_r-(int)input_r); } else if(car_state == CAR_STATE_STOPPING){ getMotorData(); if (last_state == CAR_STATE_MOVE_FORWARD) { if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2)) { rin = 0; pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter); pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter); float input_l = n_l.u; float input_r = n_r.u; proc_cmd("forward", base_pwm_l + input_l, base_pwm_r + input_r); } else{ record_controller_base(&n_r); record_controller_base(&n_l); controller_reset(&n_r); controller_reset(&n_l); car_state = CAR_STATE_IDLE; proc_cmd("forward", base_pwm_l, base_pwm_r); data_sending = 1; } } else if (last_state == CAR_STATE_MOVE_BACK) { if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2)) { rin = 0; pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter); pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter); float input_l = n_l_back.u; float input_r = n_r_back.u; proc_cmd("forward", base_pwm_l - input_l, base_pwm_r - input_r); } else{ record_controller_base(&n_r_back); record_controller_base(&n_l_back); controller_reset(&n_r_back); controller_reset(&n_l_back); car_state = CAR_STATE_IDLE; proc_cmd("forward", base_pwm_l, base_pwm_r); data_sending = 1; } } else if (last_state == CAR_STATE_MOVE_LEFT) { if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2)) { rin = 0; pid_update(&n_l_back, rin, speed_left_counter_1, distance_left_counter); pid_update(&n_r, rin, speed_right_counter_1, distance_right_counter); float input_l = n_l_back.u; float input_r = n_r.u; proc_cmd("forward", base_pwm_l - input_l, base_pwm_r + input_r); } else{ //record_controller_base(&n_r); //record_controller_base(&n_l_back); controller_reset(&n_r); controller_reset(&n_l_back); car_state = CAR_STATE_IDLE; proc_cmd("forward", base_pwm_l, base_pwm_r); data_sending = 1; } } else if (last_state == CAR_STATE_MOVE_RIGHT) { if ((speed_left_counter_1 > 2) && (speed_right_counter_1 > 2)) { rin = 0; pid_update(&n_l, rin, speed_left_counter_1, distance_left_counter); pid_update(&n_r_back, rin, speed_right_counter_1, distance_right_counter); float input_l = n_l.u; float input_r = n_r_back.u; proc_cmd("forward", base_pwm_l + input_l, base_pwm_r - input_r); } else{ //record_controller_base(&n_r_back); //record_controller_base(&n_l); controller_reset(&n_r_back); controller_reset(&n_l); car_state = CAR_STATE_IDLE; proc_cmd("forward", base_pwm_l, base_pwm_r); data_sending = 1; } } } else if(car_state == CAR_STATE_IDLE){ proc_cmd("forward", base_pwm_l, base_pwm_r); speeed_initialize(); distance_initialize(); } else{ } attachInterrupt(EXTI_Line0); attachInterrupt(EXTI_Line1); attachInterrupt(EXTI_Line2); attachInterrupt(EXTI_Line3); vTaskDelay(NEURAL_PERIOD); } }