yotaro morizumi
/
zoomy_customLibrary
my new gear...
Diff: actuator/IMC_motorDrive.cpp
- Revision:
- 21:72dcbbfeb5d8
- Parent:
- 10:b64f586efb2d
diff -r 4e0fa6fba27c -r 72dcbbfeb5d8 actuator/IMC_motorDrive.cpp --- a/actuator/IMC_motorDrive.cpp Fri Sep 30 00:04:11 2022 +0000 +++ b/actuator/IMC_motorDrive.cpp Fri Sep 30 00:07:30 2022 +0000 @@ -6,9 +6,6 @@ } void IMC_motor::IMCdrive(double target){ - if(cw_flag){ - target = -target; - } drive((int)imc_out(target)); } @@ -17,21 +14,46 @@ process_model_val = 0; } -void IMC_motor::drive(int pwm){ - float output_pwm = constrain(pwm,-200,200)/255; - if(!pwm) { - DigitalOut Moter1(motor.pin[0],0); - DigitalOut Moter2(motor.pin[1],0); - } else if(pwm < 0) { - PwmOut Moter1(motor.pin[0]); - Moter1.period_us(256); - Moter1.write(-output_pwm); - DigitalOut Moter2(motor.pin[1],0); +void IMC_motor::drive(int pwm) +{ + double output_pwm; + if(pwm < -200) { + output_pwm = -200; + } else if(pwm > 200) { + output_pwm = 200; } else { - DigitalOut Moter1(motor.pin[0],0); - PwmOut Moter2(motor.pin[1]); - Moter2.period_us(256); - Moter2.write(output_pwm); + output_pwm = pwm; + } + if(cw_flag) { + if(!pwm) { + DigitalOut Moter1(motor.pin[0],0); + DigitalOut Moter2(motor.pin[1],0); + } else if(pwm < 0) { + PwmOut Moter1(motor.pin[0]); + Moter1.period_us(256); + Moter1.write(-output_pwm/255); + DigitalOut Moter2(motor.pin[1],0); + } else { + DigitalOut Moter1(motor.pin[0],0); + PwmOut Moter2(motor.pin[1]); + Moter2.period_us(256); + Moter2.write(output_pwm/255); + } + }else{ + if(!pwm) { + DigitalOut Moter1(motor.pin[0],0); + DigitalOut Moter2(motor.pin[1],0); + } else if(pwm < 0) { + PwmOut Moter1(motor.pin[1]); + Moter1.period_us(256); + Moter1.write(-output_pwm/255); + DigitalOut Moter2(motor.pin[0],0); + } else { + DigitalOut Moter1(motor.pin[1],0); + PwmOut Moter2(motor.pin[0]); + Moter2.period_us(256); + Moter2.write(output_pwm/255); + } } }