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