Group 9 BioRobotics
/
motor_calibration
V0.1
Fork of motor_calibration by
main.cpp
- Committer:
- s1725696
- Date:
- 2018-10-26
- Revision:
- 5:32dbe204d89a
- Parent:
- 4:2ee0d20f624b
- Child:
- 6:2af62394c832
File content as of revision 5:32dbe204d89a:
#include "mbed.h" #include "QEI.h" #define SERIAL_BAUD 115200 //initial allocations DigitalOut dirpin(D4); DigitalOut dirpin2(D7); PwmOut pwmpin(D5); PwmOut pwmpin2(D6); QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING); QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING); Serial pc(USBTX,USBRX); //volatiles volatile int counts_prev=0; volatile int counts2_prev=0; volatile int count; volatile int count2; volatile float pos1=0.1; volatile float pos2=0.1; volatile float pos3=0.1; int position; //functions (2 functions not finished) int counts(){ counts_prev=count; int count=Encoder.getPulses(); return count;} int counts2(){ counts2_prev=count2; int count2=Encoder2.getPulses(); //MAAK NOG AF MET PREVIOUS COUNTS ONTHOUDEN. return count2;} int counter(int a){ int b=abs(counts2()-a); return b;} void pos_store(int a){ //store position in counts to know count location of the ends of bridge if (pos1 == 0.1){ pos1=a;} else if (pos2 == 0.1){ pos2=a;} else if (pos3 == 0.1){ pos3=a;} else {}} void translation_start(int a, float b){ //start rail motor dirpin.write(a); pwmpin = b;} void translation_stop(){ //stop rail motor pwmpin = 0;} void rotate_start(int a, float b){ //start rotation of disk dirpin2.write(a); pwmpin2 = b;} void rotate_stop(){ //stop rotation of disk pwmpin2 = 0;} //main function int main(){ pc.baud(115200); pc.printf("start\r\n"); pwmpin.period_us(60); //parameters int caltime=30; //calibration time until condition has been met, caltime=(time/0.1) float speed=0.70f; int dir=0; int counts2_end=10000; //amount of counts for 360 degrees rotation of main disk --> many revolutions of motor //move towards end for (int m=1;m<=2;m++){ pc.printf("binnen for m \r\n"); translation_start(dir,speed); //when motor counts do not change anymore, change direction int n=1; while(n==1){ /*for (n=1;caltime;n++){ if (counts()!=counts_prev){ n=0;} else {} wait(0.1);}*/ wait(5); translation_stop(); position=counts(); pc.printf("while loop \r\n"); n=2;} dir=dir+1; pc.printf("direction \r\n"); pos_store(position); pc.printf("position \r\n");} //stores position integer //Check if a full circle can be made without obstruction //Try adding a timer as well, you know how long it should take, how long does it actually take? Does that matter? int counter2=counts2(); rotate_start(dir, speed); while(1){ if (counter(counter2)>=counts2_end){ break;} else {} wait(0.01);} rotate_stop(); pos_store(counts2()); pc.printf("position 1 = %f, position 2 = %f, position 3 = %f /n",pos1,pos2,pos3); pc.printf("done"); //End sequence? }