jordy morsinkhof / Mbed 2 deprecated Biorobotics_Motor_Control

RobotArm_ Biorobotics project.

Dependencies:   PID QEI RemoteIR Servo mbed

Fork of Biorobotics_Motor_Control by Bram S

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?

UserRevisionLine numberNew 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 }