// run moter
void Motor::run(float power) {
if (power > 1) power = 1;
if (power < -1) power = -1;
if (power < 0) {
power_level->write(-power * limit);
rotate_reverse();
} else {
power_level->write(power * limit);
rotate_normal();
}
}
int main(void)
{
Systick_Init();
tick.attach(&blink1, 1000);
pwm.enable_irq();
pwm.attach(&blink2);
pwm = 0.5;
while(1)
{
}
}
void positionPID() {
angularPos = angularRes*encoderCount;
position = angularPos*spoolRadius;
error = setPoint - position;
control = Kp*error + Kd*(error-lastError);
lastError = error;
lastPosition = position;
if ((position > 22)||(position < -12)) {
_enable.write(0.00f);
} else {
if (control >= 0.00f) {
if (control > 1.00f) {
pwmControl = 1.00f;
} else {
pwmControl = control;
}
_phase.write(1);
if (pwmControl > 0.00f) {
_enable.write(pwmControl);
} else {
_enable.write(0.00f);
}
} else {
if (fabs(control) > 1.00f) {
pwmControl = 1.00f;
} else {
pwmControl = fabs(control);
}
_phase.write(0);
if (pwmControl > 0.00f) {
_enable.write(pwmControl);
} else {
_enable.write(0.00f);
}
}
}
}
void toIdle()
{
//Turn off circle LEDS
//GPIOE->ODR &= 0x00FF;
//Turn on all circle LEDS
//GPIOE->ODR |= 0xFF00;
GPIOE->ODR |= 0x2000;
//Turn off QTR
GPIOB->ODR &= ~( 1<<12 );
//GPIOD->ODR &= ~( GPIO_Pin_3 | GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_6 );
motors.dutyCycle( 0.0f, 0.0f );
}
Finger(PinName phase, PinName enable, PinName encoderPinA, PinName encoderPinB) : _phase(phase),_enable(enable),_encoderPinA(encoderPinA),_encoderPinB(encoderPinB), _encoderInterrupt(encoderPinA) {
_encoderInterrupt.rise(this, &Finger::readEncoder);
_encoderInterrupt.fall(this, &Finger::readEncoder);
_enable.period_us(20); // Set PWM frequency to 50 KHz
}
// brake
void Motor::brake(float powor) {
normal = 1;
reverse = 1;
power_level->write(powor * limit);
}
// set flequency
void Motor::set_frequency(float correct_frequency) {
power_level->period(1 / correct_frequency);
}
// brake
void Motor::brake(void) {
normal = 1;
reverse = 1;
powerLevel->write(limit);
}
// set flequency
void Motor::setFlequency(float correctFlequency) {
powerLevel->period(1 / correctFlequency);
}