Group 9 BioRobotics / Mbed 2 deprecated motor_calibration

V0.1

Dependencies:   mbed QEI

Fork of motor_calibration by Kenneth Weisbeek

main.cpp@7:32caebeb5c3a, 2018-10-31 (annotated)

Committer:
s1725696
Date:
Wed Oct 31 18:08:39 2018 +0000
Revision:
7:32caebeb5c3a
Parent:
6:2af62394c832
Child:
8:ebb645b3e15f
it works!

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
s1725696 7:32caebeb5c3a 6 DigitalOut dirpin(D4); // for translatie
s1725696 7:32caebeb5c3a 7 DigitalOut dirpin_2(D7); // for rotatie
kweisbeek 0:1045216da12e 8 PwmOut pwmpin(D5);
s1725696 6:2af62394c832 9 PwmOut pwmpin_2(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
s1725696 6:2af62394c832 15 DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
s1725696 6:2af62394c832 16 DigitalIn button_wait(SW3); // button for wait mode, on mbed
s1725696 7:32caebeb5c3a 17 DigitalIn button_demo(D2); // button for demo mode, on bioshield
s1725696 6:2af62394c832 18
s1725696 6:2af62394c832 19 // Volatiles
s1725696 6:2af62394c832 20 volatile int counts1_prev = 0;
s1725696 6:2af62394c832 21 volatile int counts2_prev = 0;
s1725696 6:2af62394c832 22 volatile int counts1;
s1725696 6:2af62394c832 23 volatile int counts2;
s1725696 6:2af62394c832 24 volatile double tower_1_position = 0.1; // the tower which he reaches first
s1725696 6:2af62394c832 25 volatile double tower_end_position = 0.1; // the tower which he reaches second
s1725696 6:2af62394c832 26 volatile double rotation_start_position = 0.1; // the position where the rotation will remain
s1725696 7:32caebeb5c3a 27 volatile double position;
kweisbeek 0:1045216da12e 28
s1725696 6:2af62394c832 29 // Functions (2 functions not finished)
s1725696 7:32caebeb5c3a 30 int Counts1(volatile int& a) // a = counts1
s1725696 6:2af62394c832 31 {
s1725696 6:2af62394c832 32 counts1_prev = a;
s1725696 6:2af62394c832 33 a = Encoder.getPulses();
s1725696 7:32caebeb5c3a 34 return a;
s1725696 6:2af62394c832 35 }
s1725696 6:2af62394c832 36
s1725696 7:32caebeb5c3a 37 int Counts2(volatile int& a) // a = counts2
s1725696 6:2af62394c832 38 {
s1725696 6:2af62394c832 39 counts2_prev = a;
s1725696 7:32caebeb5c3a 40 a = Encoder2.getPulses();
s1725696 7:32caebeb5c3a 41 return a;
s1725696 6:2af62394c832 42 }
s1725696 6:2af62394c832 43
s1725696 6:2af62394c832 44 void pos_store(int a){ //store position in counts to know count location of the ends of bridge
s1725696 6:2af62394c832 45
s1725696 6:2af62394c832 46 if (tower_1_position == 0.1){
s1725696 6:2af62394c832 47 tower_1_position = a;
s1725696 6:2af62394c832 48 }
s1725696 6:2af62394c832 49 else if (tower_end_position == 0.1){
s1725696 6:2af62394c832 50 tower_end_position = a;
s1725696 6:2af62394c832 51 }
s1725696 6:2af62394c832 52 else if (rotation_start_position == 0.1){
s1725696 6:2af62394c832 53 rotation_start_position = a;
s1725696 7:32caebeb5c3a 54 }
s1725696 6:2af62394c832 55 }
s1725696 6:2af62394c832 56
s1725696 6:2af62394c832 57 // Start translation
s1725696 6:2af62394c832 58 void translation_start(int a, float b) // a = dir , b = speed
s1725696 6:2af62394c832 59 {
s1725696 6:2af62394c832 60 dirpin.write(a);
s1725696 6:2af62394c832 61 pwmpin = b;
s1725696 6:2af62394c832 62 }
s1725696 6:2af62394c832 63
s1725696 6:2af62394c832 64 // Stop translation
s1725696 6:2af62394c832 65 void translation_stop()
s1725696 6:2af62394c832 66 {
s1725696 6:2af62394c832 67 pwmpin = 0.0;
s1725696 6:2af62394c832 68 }
s1725696 6:2af62394c832 69
s1725696 6:2af62394c832 70 // Start rotation
s1725696 7:32caebeb5c3a 71 void rotation_start(int a, float b)
s1725696 6:2af62394c832 72 {
s1725696 6:2af62394c832 73 dirpin_2.write(a);
s1725696 6:2af62394c832 74 pwmpin_2 = b;
s1725696 6:2af62394c832 75 }
s1725696 6:2af62394c832 76
s1725696 6:2af62394c832 77 // Stop rotation
s1725696 7:32caebeb5c3a 78 void rotation_stop()
s1725696 6:2af62394c832 79 {
s1725696 6:2af62394c832 80 pwmpin_2 = 0.0;
s1725696 6:2af62394c832 81 }
kweisbeek 1:53bb5928adcf 82
kweisbeek 1:53bb5928adcf 83
s1725696 6:2af62394c832 84 // main function
s1725696 6:2af62394c832 85 int main()
s1725696 6:2af62394c832 86 {
kweisbeek 3:d519ec330c31 87 pc.baud(115200);
s1725696 6:2af62394c832 88 pc.printf("Start\r\n");
kweisbeek 3:d519ec330c31 89
kweisbeek 0:1045216da12e 90 pwmpin.period_us(60);
kweisbeek 0:1045216da12e 91
s1725696 7:32caebeb5c3a 92 // parameters
s1725696 7:32caebeb5c3a 93 float speed = 0.70;
s1725696 6:2af62394c832 94 int dir = 0;
kweisbeek 0:1045216da12e 95
s1725696 6:2af62394c832 96 // translation
s1725696 7:32caebeb5c3a 97 for(int m = 1; m <= 2; m++) // to do each direction one time
s1725696 6:2af62394c832 98 {
s1725696 7:32caebeb5c3a 99 pc.printf("\r\nTranslatie loop\r\n");
s1725696 6:2af62394c832 100 translation_start(dir,speed);
s1725696 7:32caebeb5c3a 101 pc.printf("Direction = %i\r\n", dir);
s1725696 6:2af62394c832 102
s1725696 7:32caebeb5c3a 103 bool g = true; // to make a condition for the while loop
s1725696 6:2af62394c832 104 while (g == true)
s1725696 6:2af62394c832 105 {
s1725696 7:32caebeb5c3a 106 if (button_demo == false) // if button_demo is pushed, the translation should stop and change direction
s1725696 6:2af62394c832 107 {
s1725696 6:2af62394c832 108 translation_stop();
s1725696 7:32caebeb5c3a 109 position = Counts1(counts1);
s1725696 7:32caebeb5c3a 110 pos_store(Counts1(counts1)); // IS THIS NEEDED??
s1725696 7:32caebeb5c3a 111 pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
s1725696 6:2af62394c832 112 dir = dir + 1;
s1725696 7:32caebeb5c3a 113
s1725696 6:2af62394c832 114 g = false; // to end the while loop
s1725696 6:2af62394c832 115 }
s1725696 7:32caebeb5c3a 116
s1725696 6:2af62394c832 117 wait(0.01);
s1725696 6:2af62394c832 118 }
s1725696 7:32caebeb5c3a 119
s1725696 7:32caebeb5c3a 120 wait(1.5); // wait 3 seconds before next round of translation/rotation
s1725696 6:2af62394c832 121 }
s1725696 7:32caebeb5c3a 122 pc.printf("before wait\r\n");
s1725696 7:32caebeb5c3a 123 wait(10.0);
s1725696 7:32caebeb5c3a 124
s1725696 6:2af62394c832 125 // rotation
s1725696 7:32caebeb5c3a 126 rotation_start(dir, speed);
s1725696 7:32caebeb5c3a 127 pc.printf("\r\nRotatie start\r\n");
s1725696 6:2af62394c832 128
s1725696 6:2af62394c832 129 bool f = true; // condition for while loop
s1725696 6:2af62394c832 130 while(f == true)
s1725696 6:2af62394c832 131 {
s1725696 7:32caebeb5c3a 132 if (button_motorcal == false) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing
s1725696 6:2af62394c832 133 {
s1725696 7:32caebeb5c3a 134 rotation_stop();
s1725696 7:32caebeb5c3a 135
s1725696 6:2af62394c832 136 f = false; // to end the while loop
s1725696 6:2af62394c832 137 }
s1725696 7:32caebeb5c3a 138
s1725696 7:32caebeb5c3a 139 wait(0.01);
s1725696 7:32caebeb5c3a 140 }
s1725696 7:32caebeb5c3a 141
s1725696 7:32caebeb5c3a 142 pos_store(Counts2(counts2));
s1725696 7:32caebeb5c3a 143 pc.printf("position of first tower = %.1f, position of second tower = %.1f, position of rotation motor = %.1f \r\n",tower_1_position,tower_end_position,rotation_start_position);
s1725696 6:2af62394c832 144
s1725696 7:32caebeb5c3a 145 pc.printf("Motor calibration done");
s1725696 7:32caebeb5c3a 146
s1725696 7:32caebeb5c3a 147
s1725696 7:32caebeb5c3a 148 }