RobotArm_ Biorobotics project.
Dependencies: PID QEI RemoteIR Servo mbed
Fork of Biorobotics_Motor_Control by
main.cpp@0:6c0f87177797, 2017-10-16 (annotated)
- Committer:
- BramS23
- Date:
- Mon Oct 16 13:51:22 2017 +0000
- Revision:
- 0:6c0f87177797
- Child:
- 2:684d5cf2f683
yo know it;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
BramS23 | 0:6c0f87177797 | 1 | #include "mbed.h" |
BramS23 | 0:6c0f87177797 | 2 | #include "QEI.h" |
BramS23 | 0:6c0f87177797 | 3 | AnalogIn PotMeter(A0); |
BramS23 | 0:6c0f87177797 | 4 | InterruptIn Button(PTA4); |
BramS23 | 0:6c0f87177797 | 5 | |
BramS23 | 0:6c0f87177797 | 6 | PwmOut MotorThrottle1(D5); |
BramS23 | 0:6c0f87177797 | 7 | PwmOut MotorThrottle2(D6); |
BramS23 | 0:6c0f87177797 | 8 | DigitalOut MotorDirection1(D4); |
BramS23 | 0:6c0f87177797 | 9 | DigitalOut MotorDirection2(D7); |
BramS23 | 0:6c0f87177797 | 10 | |
BramS23 | 0:6c0f87177797 | 11 | QEI Encoder(D12,D13,NC,32); |
BramS23 | 0:6c0f87177797 | 12 | |
BramS23 | 0:6c0f87177797 | 13 | Serial pc(PTB17,PTB16); |
BramS23 | 0:6c0f87177797 | 14 | |
BramS23 | 0:6c0f87177797 | 15 | void ButtonPress(){ |
BramS23 | 0:6c0f87177797 | 16 | MotorDirection1 = !MotorDirection1; |
BramS23 | 0:6c0f87177797 | 17 | } |
BramS23 | 0:6c0f87177797 | 18 | |
BramS23 | 0:6c0f87177797 | 19 | |
BramS23 | 0:6c0f87177797 | 20 | int main() { |
BramS23 | 0:6c0f87177797 | 21 | pc.baud(115200); |
BramS23 | 0:6c0f87177797 | 22 | float speed = 0.0f; |
BramS23 | 0:6c0f87177797 | 23 | int pos = 0; |
BramS23 | 0:6c0f87177797 | 24 | int pos_ref = 0; |
BramS23 | 0:6c0f87177797 | 25 | int temp_ref = 0; |
BramS23 | 0:6c0f87177797 | 26 | int pos_prev = 0; |
BramS23 | 0:6c0f87177797 | 27 | pc.printf("startup"); |
BramS23 | 0:6c0f87177797 | 28 | Button.rise(&ButtonPress); |
BramS23 | 0:6c0f87177797 | 29 | |
BramS23 | 0:6c0f87177797 | 30 | while(true){ |
BramS23 | 0:6c0f87177797 | 31 | //pc.printf("PWM: %f, POS: %i, REF: %i\r\n",speed,pos,pos_ref); |
BramS23 | 0:6c0f87177797 | 32 | pos = Encoder.getPulses(); |
BramS23 | 0:6c0f87177797 | 33 | temp_ref =(int) 10000*PotMeter.read(); |
BramS23 | 0:6c0f87177797 | 34 | if(fabsf(temp_ref-pos_ref)>50){ |
BramS23 | 0:6c0f87177797 | 35 | pos_ref = temp_ref; |
BramS23 | 0:6c0f87177797 | 36 | } |
BramS23 | 0:6c0f87177797 | 37 | speed = ((float)(pos-pos_ref))/10000.0f; |
BramS23 | 0:6c0f87177797 | 38 | if (speed<0.0f){ |
BramS23 | 0:6c0f87177797 | 39 | MotorDirection1=1; |
BramS23 | 0:6c0f87177797 | 40 | speed = -speed; |
BramS23 | 0:6c0f87177797 | 41 | } |
BramS23 | 0:6c0f87177797 | 42 | else{ |
BramS23 | 0:6c0f87177797 | 43 | MotorDirection1=0; |
BramS23 | 0:6c0f87177797 | 44 | } |
BramS23 | 0:6c0f87177797 | 45 | if(speed<0.01f){ |
BramS23 | 0:6c0f87177797 | 46 | speed=0; |
BramS23 | 0:6c0f87177797 | 47 | } |
BramS23 | 0:6c0f87177797 | 48 | MotorThrottle1.write(speed); |
BramS23 | 0:6c0f87177797 | 49 | pos_prev=pos; |
BramS23 | 0:6c0f87177797 | 50 | } |
BramS23 | 0:6c0f87177797 | 51 | } |