static void REMOTE_HandleMotorMsg(int16_t speedVal, int16_t directionVal, int16_t z) {
int16_t speedL, speedR;
if(speedVal < 0)
{
directionVal *= -1;
}
//if(directionVal > MAX/2)
speedR = 3 * speedVal /2 - 7 * directionVal / 10;
speedL = 3 * speedVal /2 + 7 * directionVal / 10;
DRV_SetSpeed(speedL, speedR);
}
uint8_t REMOTE_HandleRemoteRxMessage(RAPP_MSG_Type type, uint8_t size, uint8_t *data, RNWK_ShortAddrType srcAddr, bool *handled, RPHY_PacketDesc *packet) {
#if PL_CONFIG_HAS_SHELL
uint8_t buf[48];
#endif
uint8_t val;
int16_t x, y, z;
(void)size;
(void)packet;
switch(type) {
#if PL_CONFIG_HAS_MOTOR
case RAPP_MSG_TYPE_JOYSTICK_XY: /* values are -128...127 */
{
int8_t direction, speed;
int16_t x1000, y1000;
*handled = TRUE;
direction = *data; /* get x data value */
speed = *(data+1); /* get y data value */
if (REMOTE_isVerbose) {
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"d/s: ");
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)direction);
UTIL1_chcat(buf, sizeof(buf), ',');
UTIL1_strcatNum8s(buf, sizeof(buf), (int8_t)speed);
UTIL1_strcat(buf, sizeof(buf), (unsigned char*)"\r\n");
SHELL_SendString(buf);
}
#if 0 /* using shell command */
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor L duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(x));
SHELL_ParseCmd(buf);
UTIL1_strcpy(buf, sizeof(buf), (unsigned char*)"motor R duty ");
UTIL1_strcatNum8s(buf, sizeof(buf), scaleSpeedToPercent(y));
SHELL_ParseCmd(buf);
#endif
//x1000 = scaleJoystickTo1K(x);
//y1000 = scaleJoystickTo1K(y);
if (REMOTE_useJoystick) {
REMOTE_HandleMotorMsg(direction, speed, 0); /* first param is forward/backward speed, second param is direction */
}
}
break;
#endif
case RAPP_MSG_TYPE_JOYSTICK_BTN:
*handled = TRUE;
val = *data; /* get data value */
#if PL_CONFIG_HAS_SHELL && PL_CONFIG_HAS_BUZZER && PL_CONFIG_HAS_REMOTE
if (val=='F') { /* F button, disable remote */
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 500");
REMOTE_SetOnOff(FALSE);
DRV_SetSpeed(0,0); /* turn off motors */
SHELL_SendString("Remote OFF\r\n");
} else if (val=='G') { /* center joystick button: enable remote */
SHELL_ParseCmd((unsigned char*)"buzzer buz 300 1000");
REMOTE_SetOnOff(TRUE);
DRV_SetMode(DRV_MODE_SPEED);
SHELL_SendString("Remote ON\r\n");
} else if (val=='C') { /* red 'C' button */
/*! \todo add functionality */
} else if (val=='A') { /* green 'A' button */
/*! \todo add functionality */
}
#else
*handled = FALSE; /* no shell and no buzzer? */
#endif
break;
default:
break;
} /* switch */
return ERR_OK;
}
static void REMOTE_HandleMotorMsg(int16_t direction, int16_t speedMode, int16_t z) {
#define SCALE_DOWN 30
#define MIN_VALUE 250 /* values below this value are ignored */
#define DRIVE_DOWN 1
#define SCALE_FROM_PERCENT (16383/100)
#define NORMAL_SPEED (20 * SCALE_FROM_PERCENT)
#define FAST_SPEED (80 * SCALE_FROM_PERCENT)
#define FAST_TURN (30 * SCALE_FROM_PERCENT)
if (!REMOTE_isOn) {
LED1_Off();
return;
}
LED1_On();
if(speedMode == 4) {
// left handed
LED2_On();
MAZE_SetTurnHandleLeft(TRUE);
LF_StartFollowing();
}
if(speedMode == 5) {
// right handed
LED2_On();
MAZE_SetTurnHandleLeft(FALSE);
LF_StartFollowing();
}
switch(direction) {
case 0: // straight
if(speedMode == 0){
DRV_SetSpeed(0, 0);
} else if (speedMode == 1) {
DRV_SetSpeed(NORMAL_SPEED, NORMAL_SPEED);
} else if (speedMode == 2) {
DRV_SetSpeed(FAST_SPEED,FAST_SPEED );
} else if (speedMode == 3){
DRV_SetSpeed( -NORMAL_SPEED, -NORMAL_SPEED );
}
break;
case 1: // right
if(speedMode == 0){
DRV_SetSpeed(NORMAL_SPEED, 0);
} else if (speedMode == 1) {
DRV_SetSpeed(FAST_SPEED, NORMAL_SPEED);
} else if (speedMode == 2) {
DRV_SetSpeed(FAST_SPEED,FAST_TURN );
} else if (speedMode == 3){
DRV_SetSpeed( -FAST_SPEED, -NORMAL_SPEED );
}
break;
case 2: // left
if(speedMode == 0){
DRV_SetSpeed(0, NORMAL_SPEED);
} else if (speedMode == 1) {
DRV_SetSpeed(NORMAL_SPEED, FAST_SPEED);
} else if (speedMode == 2) {
DRV_SetSpeed(FAST_TURN,FAST_SPEED );
} else if (speedMode == 3){
DRV_SetSpeed( -NORMAL_SPEED, -FAST_SPEED );
}
break;
}
/*
if (z<-900) { /* have a way to stop motor: turn FRDM USB port side up or down
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(0, 0);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
} else if ((directionVal>MIN_VALUE || directionVal<-MIN_VALUE) && (speedVal>MIN_VALUE || speedVal<-MIN_VALUE)) {
int16_t speed, speedL, speedR;
speed = speedVal/SCALE_DOWN;
if (directionVal<0) {
if (speed<0) {
speedR = speed+(directionVal/SCALE_DOWN);
} else {
speedR = speed-(directionVal/SCALE_DOWN);
}
speedL = speed;
} else {
speedR = speed;
if (speed<0) {
speedL = speed-(directionVal/SCALE_DOWN);
} else {
speedL = speed+(directionVal/SCALE_DOWN);
}
}
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(speedL*SCALE_DOWN/DRIVE_DOWN, speedR*SCALE_DOWN/DRIVE_DOWN);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), speedL);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), speedR);
#endif
} else if (speedVal>100 || speedVal<-100) { //* speed
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(speedVal, speedVal);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -speedVal/SCALE_DOWN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -speedVal/SCALE_DOWN);
#endif
} else if (directionVal>100 || directionVal<-100) { //* direction
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(directionVal/DRIVE_DOWN, -directionVal/DRIVE_DOWN);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -directionVal/SCALE_DOWN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), (directionVAl/SCALE_DOWN));
#endif
} else { //* device flat on the table?
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(0, 0);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
} */
}
static void REMOTE_HandleMotorMsg(int16_t speedVal, int16_t directionVal, int16_t z) {
#define MIN_VALUE 350 /* old: 250 - values below this value are ignored */
#define Nitro 2
#define Zuercher 4 /* gerade aus -> schnell */
if (!REMOTE_isOn) {
return;
}
if (z<-900) { /* have a way to stop motor: turn FRDM USB port side up or down */
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(0, 0);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
} else if ((directionVal>MIN_VALUE || directionVal<-MIN_VALUE) && (speedVal>MIN_VALUE || speedVal<-MIN_VALUE)) {
int16_t speed, speedL, speedR;
speed = speedVal*2;
if (directionVal<0) {
if (speed<0) {
speedR = speed+(directionVal);
} else {
speedR = speed-(directionVal);
}
speedL = speed;
} else {
speedR = speed;
if (speed<0) {
speedL = speed-(directionVal);
} else {
speedL = speed+(directionVal);
}
}
#if PL_CONFIG_HAS_DRIVE
if(NITRO){
DRV_SetSpeed(speedL*Nitro, speedR*Nitro);
}else {
DRV_SetSpeed(speedL, speedR);
}
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), speedL);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), speedR);
#endif
} else if (speedVal>100 || speedVal<-100) { /* speed */
#if PL_CONFIG_HAS_DRIVE
if(NITRO){
DRV_SetSpeed(speedVal*3*Nitro, speedVal*3*Nitro); /* Zuercher & Nitro esch chli vell */
} else {
DRV_SetSpeed(speedVal*Zuercher, speedVal*Zuercher);
}
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -speedVal/SCALE_DOWN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -speedVal/SCALE_DOWN);
#endif
} else if (directionVal>110 || directionVal<-110) { /* direction */
#if PL_CONFIG_HAS_DRIVE
if(NITRO){
DRV_SetSpeed(directionVal*Nitro, -directionVal*Nitro);
} else {
DRV_SetSpeed(directionVal, -directionVal);
}
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -directionVal/SCALE_DOWN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), (directionVAl/SCALE_DOWN));
#endif
} else { /* device flat on the table? */
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(0, 0);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
}
}
static void REMOTE_HandleMotorMsg(int16_t speedVal, int16_t directionVal, int16_t z) {
#define SCALE_DOWN 30
#define MIN_VALUE 250 /* values below this value are ignored */
#define DRIVE_DOWN 1
if (!REMOTE_isOn) {
return;
}
if (z<-900) { /* have a way to stop motor: turn FRDM USB port side up or down */
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(0, 0);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
} else if ((directionVal>MIN_VALUE || directionVal<-MIN_VALUE) && (speedVal>MIN_VALUE || speedVal<-MIN_VALUE)) {
int16_t speed, speedL, speedR;
speed = speedVal/SCALE_DOWN;
if (directionVal<0) {
if (speed<0) {
speedR = speed+(directionVal/SCALE_DOWN);
} else {
speedR = speed-(directionVal/SCALE_DOWN);
}
speedL = speed;
} else {
speedR = speed;
if (speed<0) {
speedL = speed-(directionVal/SCALE_DOWN);
} else {
speedL = speed+(directionVal/SCALE_DOWN);
}
}
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(speedL*SCALE_DOWN/DRIVE_DOWN, speedR*SCALE_DOWN/DRIVE_DOWN);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), speedL);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), speedR);
#endif
} else if (speedVal>100 || speedVal<-100) { /* speed */
#if PL_CONFIG_HAS_DRIVE
if(nitro){
DRV_SetSpeed((nitrovalue+2)*speedVal, (nitrovalue+2)*speedVal);
} else {
DRV_SetSpeed(3*speedVal, 3*speedVal);
}
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -speedVal/SCALE_DOWN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -speedVal/SCALE_DOWN);
#endif
} else if (directionVal>100 || directionVal<-100) { /* direction */
#if PL_CONFIG_HAS_DRIVE
if(nitro){
DRV_SetSpeed((1+nitrovalue)*directionVal, -directionVal*(1+nitrovalue));
} else {
DRV_SetSpeed(2*directionVal, -directionVal*2);
}
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -directionVal/SCALE_DOWN);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), (directionVAl/SCALE_DOWN));
#endif
} else { /* device flat on the table? */
#if PL_CONFIG_HAS_DRIVE
DRV_SetSpeed(0, 0);
#else
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
#endif
}
}
uint8_t DRV_ParseCommand(const unsigned char *cmd, bool *handled, const CLS1_StdIOType *io) {
uint8_t res = ERR_OK;
const unsigned char *p;
int32_t val1, val2;
if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_HELP)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive help")==0) {
DRV_PrintHelp(io);
*handled = TRUE;
} else if (UTIL1_strcmp((char*)cmd, (char*)CLS1_CMD_STATUS)==0 || UTIL1_strcmp((char*)cmd, (char*)"drive status")==0) {
DRV_PrintStatus(io);
*handled = TRUE;
} else if (UTIL1_strncmp((char*)cmd, (char*)"drive speed ", sizeof("drive speed ")-1)==0) {
p = cmd+sizeof("drive speed");
if (UTIL1_xatoi(&p, &val1)==ERR_OK) {
if (UTIL1_xatoi(&p, &val2)==ERR_OK) {
if (DRV_SetSpeed(val1, val2)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
*handled = TRUE;
} else {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
} else {
CLS1_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
res = ERR_FAILED;
}
} else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos reset", sizeof("drive pos reset")-1)==0) {
Q4CLeft_SetPos(0);
Q4CRight_SetPos(0);
if (DRV_SetPos(0, 0)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
*handled = TRUE;
} else if (UTIL1_strncmp((char*)cmd, (char*)"drive pos ", sizeof("drive pos ")-1)==0) {
p = cmd+sizeof("drive pos");
if (UTIL1_xatoi(&p, &val1)==ERR_OK) {
if (UTIL1_xatoi(&p, &val2)==ERR_OK) {
if (DRV_SetPos(val1, val2)!=ERR_OK) {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
*handled = TRUE;
} else {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
} else {
CLS1_SendStr((unsigned char*)"Wrong argument(s)\r\n", io->stdErr);
res = ERR_FAILED;
}
} else if (UTIL1_strncmp((char*)cmd, (char*)"drive mode ", sizeof("drive mode ")-1)==0) {
p = cmd+sizeof("drive mode");
if (UTIL1_strcmp((char*)p, (char*)"none")==0) {
if (DRV_SetMode(DRV_MODE_NONE)!=ERR_OK) {
res = ERR_FAILED;
}
} else if (UTIL1_strcmp((char*)p, (char*)"stop")==0) {
if (DRV_SetMode(DRV_MODE_STOP)!=ERR_OK) {
res = ERR_FAILED;
}
} else if (UTIL1_strcmp((char*)p, (char*)"speed")==0) {
if (DRV_SetMode(DRV_MODE_SPEED)!=ERR_OK) {
res = ERR_FAILED;
}
} else if (UTIL1_strcmp((char*)p, (char*)"pos")==0) {
if (DRV_SetMode(DRV_MODE_POS)!=ERR_OK) {
res = ERR_FAILED;
}
} else {
res = ERR_FAILED;
}
if (res!=ERR_OK) {
CLS1_SendStr((unsigned char*)"failed\r\n", io->stdErr);
}
*handled = TRUE;
}
return res;
}
static void StateMachine_Run(void){
int i;
int distance;
// Für Acceleration Sensor
int16_t x = MMA1_GetXmg();
WAIT1_Waitms(5);
int16_t y = MMA1_GetYmg();
switch (state) {
case START:
//SHELL_SendString((unsigned char*)"INFO: No calibration data present.\r\n");
WAIT1_Waitms(4700);
DRV_EnableDisable(1);
DRV_SetSpeed(3000,3000);
//WAIT1_Waitms(40);
//DRV_SetSpeed(3000,3000);
//state = ATTACK;
//DRV_SetSpeed(-4200,4200);
// MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), MOT_DIR_FORWARD);
//MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), MOT_DIR_FORWARD);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 30);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 30);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40);
/*int x0;
int x5;
int x0_cal;
int x5_cal;
x0 = Get_Reflectance_Values(0);
x0_cal = x0 / 15;
x5 = Get_Reflectance_Values(5);
x5_cal = x5 / 15;
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), x0_cal);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), x5_cal);
*/
//uint16_t* calib_values_pointer;
//calib_values_pointer = &SensorTimeType;
//calib_values_pointer = S1_GetVal();
state = DRIVE;
break;
case DRIVE:
DRV_SetSpeed(-4200,4200);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 40);
if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz
{
state = TURN;
break;
}
distance = US_usToCentimeters(US_Measure_us(),22);
if((distance <= 30) && (distance != 0)){
state = ATTACK;
}
/*
if((x >= 2000) || (x <= 2000))
{
state = STOP;
}
if((y >= 2000) || (y <= 2000))
{
state = STOP;
}
*/
break;
case TURN:
//SHELL_SendString((unsigned char*)"...stopping calibration.\r\n");
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), -40);
// ATTACK
DRV_SetSpeed(-5000,-5000);
WAIT1_Waitms(500);
/*
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), -40);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 40);
*/
DRV_SetSpeed(4200,-4200);
WAIT1_Waitms(300);
DRV_SetSpeed(5000,5000);
WAIT1_Waitms(50);
if((Get_Reflectance_Values(0) > 400) || (Get_Reflectance_Values(5) > 500))
{
state = DRIVE;
}
break;
case ATTACK:
DRV_SetSpeed(5500,5500);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 80);
//MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 80);
if((Get_Reflectance_Values(0) <= 100) || (Get_Reflectance_Values(5) <= 100)) // 0 = weiss / 1000 = schwarz
{
state = TURN;
}
/*
if((x >= 2000) || (x <= -2000))
{
state = STOP;
}
if((y>= 2000) || (y <= -2000))
{
state = STOP;
}
*/
break;
case STOP:
DRV_SetSpeed(0,0);
/*MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0);
MOT_SetSpeedPercent(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0);*/
/*
if((x >= -300) && (x <= 300))
{
if((y >= -300) && (y <= 300))
{
state = DRIVE;
}
}
*/
break;
} /* whitch */
}
