Group 9 BioRobotics / Mbed 2 deprecated motor_calibration

V0.1

Dependencies:   mbed QEI

Fork of motor_calibration by Kenneth Weisbeek

main.cpp@4:2ee0d20f624b, 2018-10-26 (annotated)

Committer:
kweisbeek
Date:
Fri Oct 26 10:09:03 2018 +0000
Revision:
4:2ee0d20f624b
Parent:
3:d519ec330c31
Child:
5:32dbe204d89a
V0.5;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kweisbeek 0:1045216da12e 1 #include "mbed.h"
kweisbeek 4:2ee0d20f624b 2 #include "QEI.h"
kweisbeek 3:d519ec330c31 3 #define SERIAL_BAUD 115200
kweisbeek 0:1045216da12e 4
kweisbeek 0:1045216da12e 5 //initial allocations
kweisbeek 0:1045216da12e 6 DigitalOut dirpin(D4);
kweisbeek 2:ea985bb173f6 7 DigitalOut dirpin2(D7);
kweisbeek 0:1045216da12e 8 PwmOut pwmpin(D5);
kweisbeek 2:ea985bb173f6 9 PwmOut pwmpin2(D6);
kweisbeek 0:1045216da12e 10 QEI Encoder(D12,D13,NC,64,QEI::X4_ENCODING);
kweisbeek 4:2ee0d20f624b 11 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
kweisbeek 0:1045216da12e 12
kweisbeek 3:d519ec330c31 13 Serial pc(USBTX,USBRX);
kweisbeek 1:53bb5928adcf 14
kweisbeek 0:1045216da12e 15 //volatiles
kweisbeek 4:2ee0d20f624b 16 volatile int counts_prev=0;
kweisbeek 4:2ee0d20f624b 17 volatile int counts2_prev=0;
kweisbeek 4:2ee0d20f624b 18 volatile int count;
kweisbeek 4:2ee0d20f624b 19 volatile int count2;
kweisbeek 4:2ee0d20f624b 20 volatile float pos1=0.1;
kweisbeek 4:2ee0d20f624b 21 volatile float pos2=0.1;
kweisbeek 4:2ee0d20f624b 22 volatile float pos3=0.1;
kweisbeek 4:2ee0d20f624b 23 int position;
kweisbeek 0:1045216da12e 24
kweisbeek 1:53bb5928adcf 25
kweisbeek 1:53bb5928adcf 26
kweisbeek 2:ea985bb173f6 27 //functions (2 functions not finished)
kweisbeek 0:1045216da12e 28 int counts(){
kweisbeek 4:2ee0d20f624b 29 counts_prev=count;
kweisbeek 4:2ee0d20f624b 30 int count=Encoder.getPulses();
kweisbeek 4:2ee0d20f624b 31 return count;}
kweisbeek 4:2ee0d20f624b 32 int counts2(){
kweisbeek 4:2ee0d20f624b 33 counts2_prev=count2;
kweisbeek 4:2ee0d20f624b 34 int count2=Encoder2.getPulses(); //MAAK NOG AF MET PREVIOUS COUNTS ONTHOUDEN.
kweisbeek 4:2ee0d20f624b 35 return count2;}
kweisbeek 4:2ee0d20f624b 36 int counter(int a){
kweisbeek 4:2ee0d20f624b 37 int b=abs(counts2()-a);
kweisbeek 4:2ee0d20f624b 38 return b;}
kweisbeek 1:53bb5928adcf 39
kweisbeek 4:2ee0d20f624b 40 void pos_store(int a){ //store position in counts to know count location of the ends of bridge
kweisbeek 4:2ee0d20f624b 41
kweisbeek 3:d519ec330c31 42 if (pos1 == 0.1){
kweisbeek 4:2ee0d20f624b 43 pos1=a;}
kweisbeek 3:d519ec330c31 44 else if (pos2 == 0.1){
kweisbeek 4:2ee0d20f624b 45 pos2=a;}
kweisbeek 3:d519ec330c31 46 else if (pos3 == 0.1){
kweisbeek 4:2ee0d20f624b 47 pos3=a;}
kweisbeek 4:2ee0d20f624b 48 else {}}
kweisbeek 1:53bb5928adcf 49
kweisbeek 4:2ee0d20f624b 50 void translation_start(int a, int b){ //start rail motor
kweisbeek 3:d519ec330c31 51 dirpin.write(a);
kweisbeek 4:2ee0d20f624b 52 pwmpin = b;}
kweisbeek 4:2ee0d20f624b 53 void translation_stop(){ //stop rail motor
kweisbeek 4:2ee0d20f624b 54 pwmpin = 0;}
kweisbeek 4:2ee0d20f624b 55 void rotate_start(int a, int b){ //start rotation of disk
kweisbeek 3:d519ec330c31 56 dirpin2.write(a);
kweisbeek 4:2ee0d20f624b 57 pwmpin2 = b;}
kweisbeek 4:2ee0d20f624b 58 void rotate_stop(){ //stop rotation of disk
kweisbeek 4:2ee0d20f624b 59 pwmpin2 = 0;}
kweisbeek 0:1045216da12e 60
kweisbeek 1:53bb5928adcf 61
kweisbeek 1:53bb5928adcf 62 //main function
kweisbeek 0:1045216da12e 63 int main(){
kweisbeek 3:d519ec330c31 64 pc.baud(115200);
kweisbeek 3:d519ec330c31 65 pc.printf("start\r\n");
kweisbeek 3:d519ec330c31 66
kweisbeek 0:1045216da12e 67 pwmpin.period_us(60);
kweisbeek 0:1045216da12e 68
kweisbeek 0:1045216da12e 69 //parameters
kweisbeek 0:1045216da12e 70 int caltime=30; //calibration time until condition has been met, caltime=(time/0.1)
kweisbeek 0:1045216da12e 71 float speed=0.50f;
kweisbeek 1:53bb5928adcf 72 int dir=0;
kweisbeek 4:2ee0d20f624b 73 int counts2_end=10000; //amount of counts for 360 degrees rotation of main disk --> many revolutions of motor
kweisbeek 0:1045216da12e 74
kweisbeek 0:1045216da12e 75 //move towards end
kweisbeek 4:2ee0d20f624b 76 for (int m=1;2;m++){
kweisbeek 2:ea985bb173f6 77 translation_start(dir,speed);
kweisbeek 0:1045216da12e 78 //when motor counts do not change anymore, change direction
kweisbeek 1:53bb5928adcf 79 int n=1;
kweisbeek 1:53bb5928adcf 80 while(n=1){
kweisbeek 3:d519ec330c31 81 /*for (n=1;caltime;n++){
kweisbeek 0:1045216da12e 82 if (counts()!=counts_prev){
kweisbeek 0:1045216da12e 83 n=0;}
kweisbeek 0:1045216da12e 84 else {}
kweisbeek 3:d519ec330c31 85 wait(0.1);}*/
kweisbeek 3:d519ec330c31 86 wait(5);
kweisbeek 2:ea985bb173f6 87 translation_stop();
kweisbeek 4:2ee0d20f624b 88 position=counts();}
kweisbeek 2:ea985bb173f6 89 dir=dir+1;
kweisbeek 2:ea985bb173f6 90 pos_store(position);} //stores position integer
kweisbeek 1:53bb5928adcf 91
kweisbeek 1:53bb5928adcf 92 //Check if a full circle can be made without obstruction
kweisbeek 1:53bb5928adcf 93 //Try adding a timer as well, you know how long it should take, how long does it actually take? Does that matter?
kweisbeek 4:2ee0d20f624b 94 int counter2=counts2();
kweisbeek 4:2ee0d20f624b 95 rotate_start(dir, speed);
kweisbeek 4:2ee0d20f624b 96
kweisbeek 1:53bb5928adcf 97 while(1){
kweisbeek 4:2ee0d20f624b 98 if (counter(counter2)>=counts2_end){
kweisbeek 1:53bb5928adcf 99 break;}
kweisbeek 1:53bb5928adcf 100 else {}
kweisbeek 1:53bb5928adcf 101 wait(0.01);}
kweisbeek 1:53bb5928adcf 102 rotate_stop();
kweisbeek 1:53bb5928adcf 103 pos_store(counts2());
kweisbeek 3:d519ec330c31 104 printf("position 1 = %f, position 2 = %f, position 3 = %f /n",pos1,pos2,pos3);
kweisbeek 3:d519ec330c31 105 printf("done");
kweisbeek 1:53bb5928adcf 106 //End sequence?
kweisbeek 1:53bb5928adcf 107 }
kweisbeek 0:1045216da12e 108
kweisbeek 0:1045216da12e 109
kweisbeek 0:1045216da12e 110