void QuaduinoFlight::init() {
motorsActive = false;
minPulseWidth = 1100;
maxPulseWidth = 1600;
gravityHover = 200;
frontMotorOffset = 11;
rearMotorOffset = 5;
rightMotorOffset = 7;
leftMotorOffset = 9;
consKp = 0.3;
consKi = 0.003;
consKd = 0.192;
aggKp = 0.93;
aggKi = 0.1;
aggKd = 0.2;
angleToSwap = 15;
writeMotor(FRONT, 0);
writeMotor(RIGHT, 0);
writeMotor(REAR, 0);
writeMotor(LEFT, 0);
yawPID = &PID(&yawInput, &yawOutput, &yawSetpoint, 4.0, 0.0, 0.0, DIRECT);
pitchPID = &PID(&pitchInput, &pitchOutput, &pitchSetpoint, 4.0, 0.0, 0.0, DIRECT);
rollPID = &PID(&rollInput, &rollOutput, &rollSetpoint, 4.0, 0.0, 0.0, DIRECT);
setupPidControl();
}
float getDesiredMotorPWM(float setpoint, float measurement, volatile isNewMeasurementAvailable_t* isNewMeasurementAvailable, uint8_t channel)
{
static struct persistantPIDVariables_s PIDVariables[] = {
{ 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0 } };
if (*isNewMeasurementAvailable == NEW_MEASUREMENT_AVAILABLE)
{
*isNewMeasurementAvailable = NO_NEW_MEASUREMENT_AVAILABLE;
float dt = (TFC_Ticker[2] / 10000.0f);
TFC_Ticker[2] = 0;
if(dt > 0.1f)
{
dt = 0.1f;
}
// Integral wind-up protection
if (PIDVariables[channel].PWM < MAX_PWM && PIDVariables[channel].PWM > MIN_PWM)
{
PID(setpoint, measurement, &PIDVariables[channel], INCLUDE_INTEGRAL, dt);
}
else
{
PID(setpoint, measurement, &PIDVariables[channel], NO_INTEGRAL, dt);
}
if (PIDVariables[channel].PWM < MIN_PWM)
PIDVariables[channel].PWM = MIN_PWM;
else if (PIDVariables[channel].PWM > MAX_PWM)
PIDVariables[channel].PWM = MAX_PWM;
}
return PIDVariables[channel].PWM;
}
void runPIDs(){
PID1.measuredOutput = Q15(v1) ;
PID2.measuredOutput = Q15(v2) ;
PID(&PID1); /*Call the PID controller using the new measured input */
PID(&PID2); /*Call the PID controller using the new measured input */
/*The user may place a breakpoint on "PID(&fooPID)", halt the debugger,*/
/*tweak the measuredOutput variable within the watch window */
/*and then run the debugger again */
}
static int
progwstat(Chan *c, uchar *db, int n)
{
Dir d;
Prog *p;
char *u;
Osenv *o;
if(c->qid.type&QTDIR)
error(Eperm);
acquire();
p = progpid(PID(c->qid));
if(p == nil) {
release();
error(Ethread);
}
u = up->env->user;
o = p->osenv;
if(strcmp(u, o->user) != 0 && strcmp(u, eve) != 0) {
release();
error(Eperm);
}
n = convM2D(db, n, &d, nil);
if(n == 0){
release();
error(Eshortstat);
}
if(d.mode != ~0UL)
o->pgrp->progmode = d.mode&0777;
release();
return n;
}
void PID_PosCfg(int32_t currPos, int32_t setPos, bool isLeft, PID_Config *config) {
int32_t pwm;
MOT_Direction direction=MOT_DIR_FORWARD;
MOT_MotorDevice *motHandle;
pwm = PID(currPos, setPos, config);
/* transform into motor pwm */
pwm *= 1000; /* scale PID, otherwise we need high PID constants */
if (pwm>=0) {
direction = MOT_DIR_FORWARD;
} else { /* negative, make it positive */
pwm = -pwm; /* make positive */
direction = MOT_DIR_BACKWARD;
}
/* pwm is now always positive, make sure it is within 16bit PWM boundary */
if (pwm>0xFFFF) {
pwm = 0xFFFF;
}
/* send new pwm values to motor */
if (isLeft) {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
} else {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
}
MOT_SetVal(motHandle, 0xFFFF-pwm); /* PWM is low active */
MOT_SetDirection(motHandle, direction);
MOT_UpdatePercent(motHandle, direction);
}
/* Move()
* client: client to connect to robot
* distance: x-coordinate that robot should aim for;
* angle: angle that robot should stay at;
*/
float Move(playerc_client_t *client, float distance, float angle)
{
printf("Enter Move\n");
error_x = error_tx(position2d, distance);
while(error_x > 0.1)
{
error_x = error_tx(position2d, distance);
vx = PID(error_x);
error_a = error_ta(position2d, angle);
va = PID_A(error_a);
playerc_position2d_set_cmd_vel(position2d, vx, 0, va, 1.0);
if(bumper->bumpers[0]!=0 || bumper->bumpers[1]!=0)
{
playerc_position2d_set_cmd_vel(position2d, 0.0, 0.0, 0.0, 0.0);
break;
}
playerc_client_read(client);
printf("Moving : x = %f y = %f a = %f\n", position2d->px, position2d->py, position2d->pa);
}
printf("Leave Move\n");
}
TankDrive::TankDrive(int speedPinLeft, int speedPinRight, int forwardPinRight, int reversePinRight, int forwardPinLeft, int reversePinLeft)
{
// connect motor controller pins to Arduino digital pins
// motor Left
_speedPinLeft = speedPinLeft;
_forwardPinLeft = forwardPinLeft;
_reversePinLeft = reversePinLeft;
// motor two
_speedPinRight = speedPinRight;
_forwardPinRight = forwardPinRight;
_reversePinRight = reversePinRight;
// set all the motor control pins to outputs
pinMode(speedPinLeft, OUTPUT);
pinMode(speedPinRight, OUTPUT);
pinMode(forwardPinLeft, OUTPUT);
pinMode(reversePinLeft, OUTPUT);
pinMode(forwardPinRight, OUTPUT);
pinMode(reversePinRight, OUTPUT);
_usePID=1; //Will not use PID by default.
PID MotorPID = PID();
Trapezoid TrapPath = Trapezoid();
}
int32 MotorCtrlUsePid()
{
FreecaleConfig.Config.Motor.Pid.Pid.Now = Speed.Acturally;
FreecaleConfig.Config.Motor.Pid.Pid.Target = Speed.Expect;
return PID(&FreecaleConfig.Config.Motor.Pid.Pid);
}
void PID_LineCfg(uint16_t currLine, uint16_t setLine, PID_Config *config) {
int32_t pid, speed, speedL, speedR;
MOT_Direction directionL=MOT_DIR_FORWARD, directionR=MOT_DIR_FORWARD;
pid = PID(currLine, setLine, config);
/*! \todo Apply PID values to speed values */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* turn right */
speedR = speed+pid;
speedL = speed-pid;
} else { /* turn left */
speedR = speed+pid;
speedL = speed-pid;
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speedL>0xFFFF) {
speedL = 0xFFFF;
} else if (speedL<0) {
speedL = 0;
}
if (speedR>0xFFFF) {
speedR = 0xFFFF;
} else if (speedR<0) {
speedR = 0;
}
/* send new speed values to motor */
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speedL); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), directionL);
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speedR); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), directionR);
}
void loop()
{
Bluetooth();
Serial.print("CMSET:");
Serial.print(cmset);
Serial.println("cm");
Serial.println();
delay(50); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
unsigned int uS = sonar.ping(); // Send ping, get ping time in microseconds (uS).
Serial.print("Ping: ");
cm=uS / US_ROUNDTRIP_CM;
Serial.print("high:");
Serial.print(cm); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
Serial.println("cm");
//val=analogRead(potpin);// 读取传感器的模拟值并赋值给val
//Serial.println(val);//显示val 变量
//pwmval=val/4;
PID();
analogWrite(pwmpin,tempctrl);// 输出PWM（PWM 输出最大值255）
Serial.print ("pwmval:");
Serial.println (tempctrl);
Serial.println ();
delay(500);//延时0.01 秒
lcd();
}
void fight()
{
PID();
if (g_diff > 30 || g_diff < -30) turn(g_pid);
if (getBall(0) > g_debugBall[0] && getBall(0) > getBall(1) && getBall(0) >= getBall(2) && getBall(0) >= getBall(3) && getBall(0) >= getBall(4) && getBall(0) >= getBall(5) && getBall(0) >= getBall(6) && getBall(0) > getBall(7) ) moveAngle4ch(0,0,0);
else if(getBall(1) > g_debugBall[1] && getBall(1) >= getBall(2) && getBall(1) > getBall(3) && getBall(1) > getBall(4) && getBall(1) > getBall(5) && getBall(1) > getBall(6) && getBall(1) > getBall(7))
{
//if (getPing(3) < 15) moveAngle4ch(50 , 0, 0);
if (getBall(1) > 600) moveAngle4ch(50, 0, 95);
else moveAngle4ch(85 , 0, 95);
}
else if(getBall(2) > g_debugBall[2] && getBall(2) >= getBall(3) && getBall(2) > getBall(4) && getBall(2) > getBall(5) && getBall(2) > getBall(6) && getBall(2) > getBall(7))
{
if(getBall(2) > 600)moveAngle4ch(50 , 0, 95);
else moveAngle4ch(85, 0, 95);
}
else if (getBall(6) > g_debugBall[6] && getBall(6) >= getBall(5))
{
if (getBall(6) > 600)moveAngle4ch(50 , 0, 265);
else moveAngle4ch(85, 0, 265);
}
else if(getBall(7) > g_debugBall[7] && getBall(7) > getBall(1))
{
//if(getPing(1) < 15) moveAngle4ch(50 , 0, 0);
if (getBall(7) > 600) moveAngle4ch(50, 0, 265);
else moveAngle4ch(85, 0, 265);
}
else moveAngle4ch(0,0,0);
}
void FCInitOne()
{
delay_nms(1000);
EeSave1.Solo.BadSave = 0;
EeReadTrg(&EeSave1);
EePromDeal(&FC1.Run, sizeof(FC_RUNING_Def) - CRC_LEN, &EeSave1);
if (EeSave1.Solo.BadSave)
{
uint08 tmp;
uint16 *pdata = (uint16 *)&FC1.Run;
EeSave1.Solo.BadSave = 0;
for (tmp = 0; tmp < FC_SETTING_MAX; tmp++)
{
pdata[tmp] = FCParCfg[tmp].Default;
}
}
EeWriteTrg(&EeSave1);
FC1.State.Sys.All = 0;
PID(&FC1.FCpid, &FC1.Run.FCpidPar);
FC1.Buff.RoomTempAid = 200;
FC1.Buff.DiTreat.All = 0;
}
void Program::startProgram() {
time = 0;
// Comms *c = new Comms();
// c->openSerial();
// Load config
c.loadConfig();
// Start oven controller
oven = new OvenController();
connect(oven, SIGNAL(updateTemp(float)), this, SLOT(updatePID(float)));
oven->setConfig(&c);
oven->openSerial();
// Create PID controller
pid = PID(&c);
pid.initialise();
// Create & connect timer
timer = new QTimer(this);
connect(timer, SIGNAL(timeout()), this, SLOT(updateTick()));
timer->start((1 / c.getSampleRate()) * 1000);
}
Client::Client ()
: accumulatedLag(0), zombie(false), allowedToDisconnect(true), ready(false), mute(false),
accountID(0), playerID(0), securityLevel(0), superclient(false),
name(""), waypointEffectID(0), waypointIsDisplaying(false),
pathEffectID(0), pathPath(NULL), pathIsDisplaying(false),
locationEffectID(0), locationIsDisplaying(false),cheatMask(NO_CHEAT)
{
actor = 0;
target = 0;
exchangeID = 0;
advisorPoints = 0;
lastInviteTime = 0;
spamPoints = 0;
clientnum = 0;
detectedCheatCount = 0;
nextFloodHistoryIndex = 0;
lastInventorySend = 0;
lastGlyphSend = 0;
isAdvisor = false;
isFrozen = false;
lastInviteResult = true;
hasBeenWarned = false;
hasBeenPenalized = false;
valid = false;
isBuddyListHiding = false;
// pets[0] is a special case for the players familiar.
pets.Insert(0, PID(0));
}
void PID_PosCfg(int16_t currPos, int16_t setPos, bool isLeft, PID_Config *config) {
int32_t pid, speed;
MOT_Direction direction=MOT_DIR_FORWARD;
pid = PID(currPos, setPos, config);
/* transform into motor speed */
speed = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* limit the speed */
pid = Limit(pid, -speed, speed);
if (pid<0) { /* move forward */
speed -= pid;
direction = MOT_DIR_FORWARD;
} else { /* move backward */
speed += pid;
direction = MOT_DIR_BACKWARD;
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speed>0xFFFF) {
speed = 0xFFFF;
} else if (speed<0) {
speed = 0;
}
/* send new speed values to motor */
if (isLeft) {
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_LEFT), 0xFFFF-speed); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_LEFT), direction);
} else {
MOT_SetVal(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), 0xFFFF-speed); /* PWM is low active */
MOT_SetDirection(MOT_GetMotorHandle(MOT_MOTOR_RIGHT), direction);
}
}
void PID_SpeedCfg(int32_t currSpeed, int32_t setSpeed, bool isLeft, PID_Config *config) {
int32_t speed;
MOT_Direction direction=MOT_DIR_FORWARD;
MOT_MotorDevice *motHandle;
speed = PID(currSpeed, setSpeed, config);
if (speed>=0) {
direction = MOT_DIR_FORWARD;
} else { /* negative, make it positive */
speed = -speed; /* make positive */
direction = MOT_DIR_BACKWARD;
}
/* speed shall be positive here, make sure it is within 16bit PWM boundary */
if (speed>0xFFFF) {
speed = 0xFFFF;
}
/* send new speed values to motor */
if (isLeft) {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
} else {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
}
MOT_SetVal(motHandle, 0xFFFF-speed); /* PWM is low active */
MOT_SetDirection(motHandle, direction);
MOT_UpdatePercent(motHandle, direction);
}
fractional pidHWRun(tPID* controller, fractional feedback) {
controller->measuredOutput = feedback;
PID(controller);
return controller->controlOutput;
}
void Keeper()
{
static int g_loop = 0;
getMaxball();
PID();
if (g_diff > 40 || g_diff < -40) turn(g_pid);
else
{
g_integral == 0;
if(g_no == 0 && g_ball[0] > 300)
{
moveAngle( 80, 0 , 0);
kick();
}
else if(g_no == 1 && g_ball[1] > 300) moveAngle( 80 , 0 , 270 );
else if(g_no == 2 && g_ball[2] > 300) moveAngle( 80 , 0 , 270 );
else if(g_no == 3 && g_ball[3] > 300) moveAngle( 50 , 0 , 315 );
else if(g_no == 4 && g_ball[4] > 300) moveAngle( 0 , 0 , 180 );
else if(g_no == 5 && g_ball[5] > 300) moveAngle( 50 , 0 , 45 );
else if(g_no == 6 && g_ball[6] > 300) moveAngle( 80 , 0 , 80 );
else if(g_no == 7 && g_ball[7] > 300) moveAngle( 80 , 0 , 80 );
else goHome();
}
g_loop++;
}
int is_provides_true(Node n) {
int i;
int num_resources = 0;
peos_resource_t* resources;
peos_resource_t* proc_resources;
peos_context_t* context = peos_get_context(PID(n));
resources = get_resource_list_action_provides(PID(n), n->name, &num_resources);
if (context && context->num_resources > 0) {
proc_resources = (peos_resource_t *) calloc(context->num_resources, sizeof(peos_resource_t));
for (i = 0; i < context->num_resources; i++) {
strcpy(proc_resources[i].name, context->resources[i].name);
strcpy(proc_resources[i].value, context->resources[i].value);
}
eval_resource_list(&proc_resources, context->num_resources);
fill_resource_list_value(proc_resources, context->num_resources, &resources, num_resources);
}
return eval_predicate(resources, num_resources, n->provides);
}
SlottedPage* SSI::operator->() const {
if (page == nullptr) {
SSI* that = const_cast<SSI*>(this);
that->frame = &segment->fixPage(PID(segment->getID(), pageId), false);
that->page = new SlottedPage(segment, *frame);
}
return page;
}
void Set_Speed(int v)
{
v_PID = PID(v);
Motor_PWM = Motor_PWM+v_PID;
if(Motor_PWM > Speed_Max) Motor_PWM = Speed_Max;
else if(Motor_PWM < -Speed_Max) Motor_PWM = -Speed_Max;
/////////////////////////////////////////////////////////////////////////////
SetSpeed(Motor_PWM);
}
void Client::SetFamiliar( gemActor *familiar )
{
if ( familiar )
{
pets[0] = familiar->GetPID();
}
else
{
pets[0] = PID(0);
}
}
vm_exit_code set_act_state_graph(Graph g, char *action, vm_act_state state)
{
int error;
Node n;
switch(state)
{
case ACT_DONE:
return action_done(g,action);
case ACT_READY:
n = find_node(g,action);
if (n!=NULL) {
set_node_state(n, ACT_READY, &error);
if (!error)
return VM_CONTINUE;
}
return VM_INTERNAL_ERROR;
case ACT_RUN:
if (action_run(g,action) > 0)
return VM_CONTINUE;
return VM_INTERNAL_ERROR;
case ACT_NONE:
n = find_node(g,action);
if (n != NULL) {
set_node_state(n, ACT_NONE, &error);
if (!error)
return VM_CONTINUE;
}
return VM_INTERNAL_ERROR;
case ACT_SUSPEND:
n = find_node(g,action);
if (n != NULL) {
set_node_state(n, ACT_SUSPEND, &error);
if (!error)
return VM_CONTINUE;
}
return VM_INTERNAL_ERROR;
case ACT_ABORT:
n = find_node(g,action);
if (n != NULL) {
if (STATE(n) == ACT_RUN) {
set_node_state(n, ACT_NONE, &error);
if (error)
return VM_INTERNAL_ERROR;
update_process_state(PID(n));
}
return VM_CONTINUE;
}
return VM_INTERNAL_ERROR;
default :
fprintf(stderr, "Error Changing Action : Invalid Action State\n");
return -1;
}
}
Autopilot::Autopilot(FreeBoardModel* model) {
this->model = model;
autopilotTargetHeading = model->getAutopilotTargetHeading() + 720;
autopilotCurrentHeading = autopilotTargetHeading + model->getAutopilotOffCourse();
autopilotRudderCommand = model->getAutopilotRudderCommand();
lastDirection=true;
this->headingPid = PID(&autopilotCurrentHeading, &autopilotRudderCommand, &autopilotTargetHeading, P_Param, I_Param, D_Param, REVERSE);
headingPid.SetOutputLimits(0.0, 66.0); //output limits
headingPid.SetSampleTime(100);
}
QString TSPacket::toString() const {
QString str;
str.append("TSPacket @0x%1 ").arg(long(&data()[0]),0,16);
str.append("raw: 0x%1 0x%2 0x%3 0x%4\n").arg(int(data()[0]),0,16).
arg(int(data()[1]),0,16).arg(int(data()[2]),0,16).arg(int(data()[3]),0,16);
str.append(" inSync: %1\n").arg( HasSync());
str.append(" transportError: %1\n").arg( TransportError());
str.append(" payloadStart: %1\n").arg( PayloadStart() );
str.append(" priority: %1\n").arg( Priority() );
str.append(" pid: %1\n").arg( PID() );
str.append(" scrampled: %1\n").arg( ScramplingControl() );
str.append(" adaptationFieldControl: %1\n").arg( AdaptationFieldControl() );
str.append(" continuityCounter: %1\n").arg( ContinuityCounter() );
return str;
}
static void *thread_start(void *arg)
{
void (*F) (void*);
void *args;
F = ((thread_args_t *)arg)->F;
args = ((thread_args_t *)arg)->args;
#ifdef HAVE_GETTID
((thread_args_t *)arg)->tid = PID();
#endif
ReleaseSem(&((thread_args_t *)arg)->mutex);
pthread_detach(pthread_self());
F(args);
return NULL;
}
void CPIDCreateDlg::OnGridEndEdit(NMHDR *pNotifyStruct, LRESULT* pResult)
{
pids.clear();
for (int row = 1; row < m_cPidTable.GetRowCount(); ++row) {
CString startTemp = m_cPidTable.GetItemText(row, 1);
CString targetTemp = m_cPidTable.GetItemText(row, 2);
CString kp = m_cPidTable.GetItemText(row, 3);
CString kd = m_cPidTable.GetItemText(row, 4);
CString ki = m_cPidTable.GetItemText(row, 5);
pids.push_back( PID( _wtof(startTemp), _wtof(targetTemp), _wtof(kp), _wtof(kd), _wtof(ki) ) );
}
*pResult = 0;
}
S32
MotorPID(S32 target, U8 motor)
{
static S32 integrale[] = {0, 0, 0, 0};
static S32 lastError[] = {0, 0, 0, 0};
static S32 lastTarget[] = {0, 0, 0, 0};
if(lastTarget[motor] != target)
{
integrale[motor] = 0;
lastError[motor] = 0;
lastTarget[motor] = target;
}
register S32 current = nxt_motor_get_count(motor);
register S32 getSpeed = PID(target, current, &integrale[motor], &lastError[motor]);
nxt_motor_set_speed(motor, getSpeed, 0);
return getSpeed;
}
void CPIDCreateDlg::OnBnClickedButtonPidCreateAdd()
{
if( !UpdateData() )
return;
if( m_cStartTemp == m_cTargetTemp )
{
AfxMessageBox(L"시작온도와 타겟온도는 같은 값이 될 수 없습니다.");
return;
}
int pidSize = pids.size();
for(int i=0; i<pidSize; ++i){
PID pid = pids[i];
if( pid.startTemp == m_cStartTemp &&
pid.targetTemp == m_cTargetTemp ){
AfxMessageBox(L"이미 존재하는 pid parameter 입니다.");
return;
}
}
// Row size 를 변경한다
m_cPidTable.SetRowCount(pidSize+2);
// 현재 값을 저장한다.
// 제거할 수 있는 콤보 박스에도 넣어준다.
pids.push_back( PID(m_cStartTemp, m_cTargetTemp, m_cKp, m_cKi, m_cKd) );
CString label;
label.Format(L"PID #%d", pidSize+1);
m_cRemovalList.AddString(label);
initPidTable();
loadPidTable();
m_cStartTemp = m_cTargetTemp = 25;
m_cKp = m_cKd = m_cKi = 0;
// 초기화된 변수 값을 control 에 넘겨준다.
UpdateData(false);
}
void PID_PosCfg(int32_t currPos, int32_t setPos, bool isLeft, PID_Config *config) {
int32_t speed, val;
MOT_Direction direction=MOT_DIR_FORWARD;
MOT_MotorDevice *motHandle;
int error;
#define POS_FILTER 5
error = setPos-currPos;
if (error>-POS_FILTER && error<POS_FILTER) { /* avoid jitter around zero */
setPos = currPos;
}
speed = PID(currPos, setPos, config);
/* transform into motor speed */
speed *= 1000; /* scale PID, otherwise we need high PID constants */
if (speed>=0) {
direction = MOT_DIR_FORWARD;
} else { /* negative, make it positive */
speed = -speed; /* make positive */
direction = MOT_DIR_BACKWARD;
}
/* speed is now always positive, make sure it is within 16bit PWM boundary */
if (speed>0xFFFF) {
speed = 0xFFFF;
}
#if 1
/* limit speed to maximum value */
val = ((int32_t)config->maxSpeedPercent)*(0xffff/100); /* 100% */
speed = Limit(speed, -val, val);
#else
/* limit speed to maximum value */
speed = (speed*config->maxSpeedPercent)/100;
#endif
/* send new speed values to motor */
if (isLeft) {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_LEFT);
} else {
motHandle = MOT_GetMotorHandle(MOT_MOTOR_RIGHT);
}
MOT_SetVal(motHandle, 0xFFFF-speed); /* PWM is low active */
MOT_SetDirection(motHandle, direction);
MOT_UpdatePercent(motHandle, direction);
}
