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);
}
/*
* Application entry point.
*/
int main(void) {
/*
* Shell thread
*/
Thread *shelltp = NULL;
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
/*
* Activate custom stuff
*/
// mypwmInit();
// myADCinit();
// mySPIinit();
motorInit();
/*
* Activates the USB driver and then the USB bus pull-up on D+.
*/
// myUSBinit();
/*
* Main loop, does nothing except spawn a shell when the old one was terminated
*/
while (TRUE) {
setMotorData(1300,1490);
if (!shelltp && isUsbActive())
{
shelltp = shellCreate(&shell_cfg1, SHELL_WA_SIZE, NORMALPRIO);
}
else if (chThdTerminated(shelltp)) {
chThdRelease(shelltp); /* Recovers memory of the previous shell. */
shelltp = NULL; /* Triggers spawning of a new shell. */
}
chThdSleepMilliseconds(1000);
}
}
void handleSpecialEvents(short* enc){
static uint8_t oldTagValue = None;
static short elapsedEnc[2] = {0,0};
uint8_t resetAdjustSpeed = 0;
//Check for Error
if( tagValue & Error ){
//Report Error, Clear Flag, and Do Nothing
printw_err("Invalid RFID Tag!\n");
disableTag(Error);
lineFound = FALSE;
return;
}
//Depending on tag values adjust motor speed
if( tagValue != oldTagValue ){
lineFound = FALSE;
//Do first occurence actions
uint8_t diff = tagValue & ~oldTagValue;
//printw("diff = %X\n",diff);
if( diff & SpeedUp){
//Disable SPD DOWN
disableTag(SlowDown);
resetAdjustSpeed = 1;
}
if( diff & SlowDown ){
//Disable SPD UP
disableTag(SpeedUp);
resetAdjustSpeed = 1;
}
if( diff & GoLeft ){
//Disable RIGHT
disableTag(GoRight);
elapsedEnc[0] = 0;
elapsedEnc[1] = 0;
}
if( diff & GoRight ){
//Disable LEFT
disableTag(GoLeft);
elapsedEnc[0] = 0;
elapsedEnc[1] = 0;
}
if( diff & EndZone){
disableTag(SpeedUp);
disableTag(SlowDown);
disableTag(EndZone);
}
}
if( tagValue & SpeedUp ){ //SPD UP
setState("Go Fast");
//adjust to target
adjustSpeed(oldEnc[0], oldEnc[1], MAX_SPEED, resetAdjustSpeed);
}
if( tagValue & SlowDown ){ //SPD DOWN
setState("Go Slow");
//adjust to target
adjustSpeed(oldEnc[0], oldEnc[1], MIN_SPEED, resetAdjustSpeed);
}
if( tagValue & GoLeft ){ //LEFT
setState("Go Left");
//Update Encoder counts
elapsedEnc[0] += enc[0];
elapsedEnc[1] += enc[1];
//Check angle
float angle = enc2Ang(elapsedEnc[0], elapsedEnc[1]);
switch( doTurn(angle,MIN_TURN_ANGLE,MAX_TURN_ANGLE) ){
case 0: //Continue
setMotorData(&motorData,32,96); //Half Speed 0-Radius Left Turn
break;
case 1: //Done, Proceed
disableTag(GoLeft);
break;
case 2: //Done, Blocked
disableTag(GoLeft);
enableTag(GoRight);
//TODO: Handle stuck at corner situation
break;
}
}
if( tagValue & GoRight ){ //RIGHT
setState("Go Right");
//Update Encoder counts
elapsedEnc[0] += enc[0];
elapsedEnc[1] += enc[1];
//Check angle
float angle = -enc2Ang(elapsedEnc[0], elapsedEnc[1]);
switch( doTurn(angle,MIN_TURN_ANGLE,MAX_TURN_ANGLE) ){
case 0: //Continue
setMotorData(&motorData,96,32); //Half Speed 0-Radius Right Turn
break;
case 1: //Done, Proceed
disableTag(GoRight);
break;
case 2: //Done, Blocked
disableTag(GoRight);
enableTag(GoLeft);
//TODO: Handle stuck at corner situation
break;
}
}
if( tagValue & Finish ){ //FIN -- AT END TO ENSURE STOPPING IN EVENT OF OTHER TAGS
//Stop if found line
setMotorData(&motorData,64,64);
//else slow down?
}
oldTagValue = tagValue;
return;
}
void stateMachine(){
setMotorData(&motorData,64,64);
int back_diff = LEFT_BACK_IR-RIGHT_BACK_IR;
transition_state:
switch(curState){
case straight:
//First lets see if we need to make a turn
if(chkDist (dc,dc,50,50,dc,dc)){
if(RIGHT_BACK_IR > LEFT_BACK_IR)
curDir = right;
else
curDir = left;
curState = ninety;
goto transition_state;
}
if(LEFT_FRONT_IR<13){
curDir = right;
curState = turn;
goto transition_state;
}
else if(RIGHT_FRONT_IR<13){
curDir = left;
curState = turn;
goto transition_state;
}
else if(SONAR_LEFT<11){
setMotorData(&motorData,speedStop-15,speedStop-25);
}
else if(SONAR_RIGHT<11){
setMotorData(&motorData,speedStop-25,speedStop-10);
}
else if(LEFT_FRONT_IR<18){
setMotorData(&motorData,speedFast+3,speedStop);
}
else if(RIGHT_FRONT_IR<18){
setMotorData(&motorData,speedStop,speedFast+3);
}
else if(SONAR_LEFT<65){
setMotorData(&motorData,speedFast,speedStop);
}
else if(SONAR_RIGHT<65){
setMotorData(&motorData,speedStop,speedFast);
}
else if(back_diff<-8 && chkDist (dc,dc,dc,dc,35,35) ){
setMotorData(&motorData,speedFast,speedStop);
}
else if(back_diff>8 && chkDist (dc,dc,dc,dc,35,35) ){
setMotorData(&motorData,speedStop,speedFast);
}
else if(LEFT_FRONT_IR<RIGHT_FRONT_IR){
setMotorData(&motorData,speedFast,speedFast-2);
}
else if(RIGHT_FRONT_IR<LEFT_FRONT_IR){
setMotorData(&motorData,speedFast-2,speedFast);
}
else{
setMotorData(&motorData,speedFast,speedFast);
}
break;
case ninety:
//printw("doing a ninety\n");
//keep turning until the front sensors read a larg val
if (!chkDist(dc,dc,65,65,dc,dc)){
curState = straight;
curDir = none;
break;
}
if(curDir==left){
setMotorData(&motorData,speedStop-(speedMed-speedStop)-5,speedMed);
}
else if(curDir==right){
setMotorData(&motorData,speedMed,speedStop-(speedMed-speedStop)-5);
}
break;
case turn:
//printw("doing a turn\n");
if(curDir==right){
setMotorData(&motorData,speedMed+2,speedStop-(speedMed-speedStop)-8);
if(LEFT_FRONT_IR>11)
curState = straight;
}
else if(curDir==left){
setMotorData(&motorData,speedStop-(speedMed-speedStop)-8,speedMed+2);
if(RIGHT_FRONT_IR>11)
curState = straight;
}
break;
}
}
// 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 test(){
setMotorData(-250,1490);
}
/*
* Application entry point.
*/
int main(void) {
int8_t accelData[2]={0,0}; // Discovery Board's Accelerometer
uint8_t receivedBuff[4]={0,0,0,0}; // Received request/information from PandaBoard
uint8_t sentData[4] = {0,0,0,0}; // Returned Information (reply) to the PandaBoard
float imuData[7]={0,0,0,0,0,0,0}; // IMU calculated data based on Razor Boad
int* razorInfo; // Razor Board Data
int steering = 0;
int speed = 0;
int ir_data[3]={0,0,0};
int16_t us_data[3]={0,0,0};
/*
* System initializations.
* - HAL initialization, this also initializes the configured device drivers
* and performs the board-specific initializations.
* - Kernel initialization, the main() function becomes a thread and the
* RTOS is active.
*/
halInit();
chSysInit();
mypwmInit();
// Initializing Motor
motorInit();
// Initializing IR Thread
ADCinit();
// Initializing US Thread
// myUltrasonicInit();
// Initializing Discovery Board's Accelerometer
//mySPIinit();
// Initializing Razor Board
myRazorInit();
// Activates the USB driver and then the USB bus pull-up on D+.
myUSBinit();
// Initializing IMU Calculations.
initIMU();
//Starting the usb configuration
sdStart(&SDU1,&portConfig2);
char receivedInfo[11];
/*
* Main loop, it takes care of reciving the requests from Panda Board using USB protocol,
* and reply with the requested data.
*/
while (TRUE) {
receivedInfo[0]='T';
sdRead(&SDU1, receivedInfo, 10);
// getImuValues(imuData);
// getAccel(accelData);
// getIR(ir_data);
// getUS(us_data);
if(receivedInfo[0] != 'T'){
receivedInfo[11]='\0';
parse(receivedInfo);
//setMotorData(-(rcvData[1]-28),rcvData[2]-2);
setMotorData(rcvData[1],1550);
translate(rcvData[0],ir_data,us_data,razorInfo,imuData,accelData,sentData);
sdWrite(&SDU1, sentData, 4);
}
}
}
