Group 9 BioRobotics / Mbed 2 deprecated motor_calibration

V0.1

Dependencies:   mbed QEI

Fork of motor_calibration by Kenneth Weisbeek

main.cpp@6:2af62394c832, 2018-10-31 (annotated)

Committer:
s1725696
Date:
Wed Oct 31 15:34:05 2018 +0000
Revision:
6:2af62394c832
Parent:
5:32dbe204d89a
Child:
7:32caebeb5c3a
complete with buttons etc

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);
s1725696 6:2af62394c832 7 DigitalOut dirpin_2(D7);
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 6:2af62394c832 17 DigitalIn button_demo(D6); // 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
kweisbeek 4:2ee0d20f624b 27 int position;
kweisbeek 0:1045216da12e 28
s1725696 6:2af62394c832 29 // Functions (2 functions not finished)
s1725696 6:2af62394c832 30 void 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 6:2af62394c832 34 }
s1725696 6:2af62394c832 35
s1725696 6:2af62394c832 36 int counts2(volatile int& a) // a = counts2
s1725696 6:2af62394c832 37 {
s1725696 6:2af62394c832 38 counts2_prev = a;
s1725696 6:2af62394c832 39 a = Encoder2.getPulses(); //MAAK NOG AF MET PREVIOUS COUNTS ONTHOUDEN.
s1725696 6:2af62394c832 40 }
s1725696 6:2af62394c832 41
s1725696 6:2af62394c832 42 int counter(int a) // a = counts2, b = total amount of counts
s1725696 6:2af62394c832 43 {
s1725696 6:2af62394c832 44 int b = abs(counts2(counts2) - a);
s1725696 6:2af62394c832 45 return b;
s1725696 6:2af62394c832 46 }
s1725696 6:2af62394c832 47
s1725696 6:2af62394c832 48 void pos_store(int a){ //store position in counts to know count location of the ends of bridge
s1725696 6:2af62394c832 49
s1725696 6:2af62394c832 50 if (tower_1_position == 0.1){
s1725696 6:2af62394c832 51 tower_1_position = a;
s1725696 6:2af62394c832 52 }
s1725696 6:2af62394c832 53 else if (tower_end_position == 0.1){
s1725696 6:2af62394c832 54 tower_end_position = a;
s1725696 6:2af62394c832 55 }
s1725696 6:2af62394c832 56 else if (rotation_start_position == 0.1){
s1725696 6:2af62394c832 57 rotation_start_position = a;
s1725696 6:2af62394c832 58 }
s1725696 6:2af62394c832 59 else {}
s1725696 6:2af62394c832 60
s1725696 6:2af62394c832 61 }
s1725696 6:2af62394c832 62
s1725696 6:2af62394c832 63 // Start translation
s1725696 6:2af62394c832 64 void translation_start(int a, float b) // a = dir , b = speed
s1725696 6:2af62394c832 65 {
s1725696 6:2af62394c832 66 dirpin.write(a);
s1725696 6:2af62394c832 67 pwmpin = b;
s1725696 6:2af62394c832 68 }
s1725696 6:2af62394c832 69
s1725696 6:2af62394c832 70 // Stop translation
s1725696 6:2af62394c832 71 void translation_stop()
s1725696 6:2af62394c832 72 {
s1725696 6:2af62394c832 73 pwmpin = 0.0;
s1725696 6:2af62394c832 74 }
s1725696 6:2af62394c832 75
s1725696 6:2af62394c832 76 // Start rotation
s1725696 6:2af62394c832 77 void rotate_start(int a, float b)
s1725696 6:2af62394c832 78 {
s1725696 6:2af62394c832 79 dirpin_2.write(a);
s1725696 6:2af62394c832 80 pwmpin_2 = b;
s1725696 6:2af62394c832 81 }
s1725696 6:2af62394c832 82
s1725696 6:2af62394c832 83 // Stop rotation
s1725696 6:2af62394c832 84 void rotate_stop()
s1725696 6:2af62394c832 85 {
s1725696 6:2af62394c832 86 pwmpin_2 = 0.0;
s1725696 6:2af62394c832 87 }
kweisbeek 1:53bb5928adcf 88
kweisbeek 1:53bb5928adcf 89
s1725696 6:2af62394c832 90 // main function
s1725696 6:2af62394c832 91 int main()
s1725696 6:2af62394c832 92 {
kweisbeek 3:d519ec330c31 93 pc.baud(115200);
s1725696 6:2af62394c832 94 pc.printf("Start\r\n");
kweisbeek 3:d519ec330c31 95
kweisbeek 0:1045216da12e 96 pwmpin.period_us(60);
kweisbeek 0:1045216da12e 97
s1725696 6:2af62394c832 98 // parameters
s1725696 6:2af62394c832 99 int caltime = 30; // calibration time until condition has been met, caltime=(time/0.1)
s1725696 6:2af62394c832 100 float speed = 0.70f;
s1725696 6:2af62394c832 101 int dir = 0;
kweisbeek 0:1045216da12e 102
s1725696 6:2af62394c832 103 // translation
s1725696 6:2af62394c832 104 for(m = 1; m <= 2; m++) // to do each direction one time
s1725696 6:2af62394c832 105 {
s1725696 6:2af62394c832 106 translation_start(dir,speed);
s1725696 6:2af62394c832 107
s1725696 6:2af62394c832 108 bool g = 1; // to make a condition for the while loop
s1725696 6:2af62394c832 109 while (g == true)
s1725696 6:2af62394c832 110 {
s1725696 6:2af62394c832 111 if (button_demo == true) // if button_demo is pushed, the translation should stop and change direction
s1725696 6:2af62394c832 112 {
s1725696 6:2af62394c832 113 translation_stop();
s1725696 6:2af62394c832 114 position = counts1();
s1725696 6:2af62394c832 115 dir = dir + 1;
s1725696 6:2af62394c832 116 pc.printf("direction = %i\r\n", dir);
s1725696 6:2af62394c832 117 pos_store(position); // IS THIS NEEDED??
s1725696 6:2af62394c832 118 pc.printf("position of first tower= %f, position of second tower = %f, position of rotation motor = %f /n",tower_1_position,tower_end_position,rotation_start_position);
s1725696 6:2af62394c832 119 g = false; // to end the while loop
s1725696 6:2af62394c832 120 }
s1725696 6:2af62394c832 121 else{}
s1725696 6:2af62394c832 122
s1725696 6:2af62394c832 123 wait(0.01);
s1725696 6:2af62394c832 124 }
s1725696 6:2af62394c832 125 }
s1725696 6:2af62394c832 126
s1725696 6:2af62394c832 127 // rotation
s1725696 6:2af62394c832 128 rotate_start(dir, speed);
s1725696 6:2af62394c832 129
s1725696 6:2af62394c832 130 bool f = true; // condition for while loop
s1725696 6:2af62394c832 131 while(f == true)
s1725696 6:2af62394c832 132 {
s1725696 6:2af62394c832 133 if (button_motorcal == true) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing
s1725696 6:2af62394c832 134 {
s1725696 6:2af62394c832 135 rotate_stop();
s1725696 6:2af62394c832 136 pos_store(counts2());
s1725696 6:2af62394c832 137 pc.printf("position of first tower= %f, position of second tower = %f, position of rotation motor = %f /n",tower_1_position,tower_end_position,rotation_start_position);
s1725696 6:2af62394c832 138 f = false; // to end the while loop
s1725696 6:2af62394c832 139 }
s1725696 6:2af62394c832 140 else {}
s1725696 6:2af62394c832 141
s1725696 6:2af62394c832 142 pc.printf("done");
kweisbeek 1:53bb5928adcf 143
kweisbeek 1:53bb5928adcf 144 //End sequence?
kweisbeek 1:53bb5928adcf 145 }
s1725696 6:2af62394c832 146
s1725696 6:2af62394c832 147
s1725696 6:2af62394c832 148 /* stuff deleted
s1725696 6:2af62394c832 149
s1725696 6:2af62394c832 150 for (n=1;caltime;n++){
s1725696 6:2af62394c832 151 if (counts()!=counts_prev){
s1725696 6:2af62394c832 152 n=0;}
s1725696 6:2af62394c832 153 else {}
s1725696 6:2af62394c832 154 wait(0.1);}
s1725696 6:2af62394c832 155
s1725696 6:2af62394c832 156 */
kweisbeek 0:1045216da12e 157
kweisbeek 0:1045216da12e 158
kweisbeek 0:1045216da12e 159