RobotArm_ Biorobotics project.
Dependencies: PID QEI RemoteIR Servo mbed
Fork of Biorobotics_Motor_Control by
Diff: main.cpp
- Revision:
- 0:6c0f87177797
- Child:
- 2:684d5cf2f683
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Oct 16 13:51:22 2017 +0000 @@ -0,0 +1,51 @@ +#include "mbed.h" +#include "QEI.h" +AnalogIn PotMeter(A0); +InterruptIn Button(PTA4); + +PwmOut MotorThrottle1(D5); +PwmOut MotorThrottle2(D6); +DigitalOut MotorDirection1(D4); +DigitalOut MotorDirection2(D7); + +QEI Encoder(D12,D13,NC,32); + +Serial pc(PTB17,PTB16); + +void ButtonPress(){ + MotorDirection1 = !MotorDirection1; +} + + +int main() { + pc.baud(115200); + float speed = 0.0f; + int pos = 0; + int pos_ref = 0; + int temp_ref = 0; + int pos_prev = 0; + pc.printf("startup"); + Button.rise(&ButtonPress); + + while(true){ + //pc.printf("PWM: %f, POS: %i, REF: %i\r\n",speed,pos,pos_ref); + pos = Encoder.getPulses(); + temp_ref =(int) 10000*PotMeter.read(); + if(fabsf(temp_ref-pos_ref)>50){ + pos_ref = temp_ref; + } + speed = ((float)(pos-pos_ref))/10000.0f; + if (speed<0.0f){ + MotorDirection1=1; + speed = -speed; + } + else{ + MotorDirection1=0; + } + if(speed<0.01f){ + speed=0; + } + MotorThrottle1.write(speed); + pos_prev=pos; + } +}