Group 9 BioRobotics / Mbed 2 deprecated motor_calibration

V0.1

Dependencies:   mbed QEI

Fork of motor_calibration by Kenneth Weisbeek

main.cpp@10:4d5af6e84c7a, 2018-10-31 (annotated)

Committer:
s1725696
Date:
Wed Oct 31 18:36:38 2018 +0000
Revision:
10:4d5af6e84c7a
Parent:
9:6672bbf14f7a 
nvm, last version it is

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
s1725696 9:6672bbf14f7a 5 /* THIS IS IMPLEMENTED IN THE BIG CODE, DO NOT CHANGE IT HERE, BUT THERE!!! */
s1725696 9:6672bbf14f7a 6
kweisbeek 0:1045216da12e 7 //initial allocations
s1725696 10:4d5af6e84c7a 8 DigitalOut dirpin_1(D4); // for translatie
s1725696 7:32caebeb5c3a 9 DigitalOut dirpin_2(D7); // for rotatie
s1725696 10:4d5af6e84c7a 10 PwmOut pwmpin_1(D5);
s1725696 6:2af62394c832 11 PwmOut pwmpin_2(D6);
s1725696 8:ebb645b3e15f 12 QEI Encoder1(D12,D13,NC,64,QEI::X4_ENCODING);
kweisbeek 4:2ee0d20f624b 13 QEI Encoder2(D10,D11,NC,64,QEI::X4_ENCODING);
kweisbeek 0:1045216da12e 14
kweisbeek 3:d519ec330c31 15 Serial pc(USBTX,USBRX);
kweisbeek 1:53bb5928adcf 16
s1725696 6:2af62394c832 17 DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
s1725696 6:2af62394c832 18 DigitalIn button_wait(SW3); // button for wait mode, on mbed
s1725696 7:32caebeb5c3a 19 DigitalIn button_demo(D2); // button for demo mode, on bioshield
s1725696 6:2af62394c832 20
s1725696 6:2af62394c832 21 // Volatiles
s1725696 6:2af62394c832 22 volatile int counts1_prev = 0;
s1725696 6:2af62394c832 23 volatile int counts2_prev = 0;
s1725696 6:2af62394c832 24 volatile int counts1;
s1725696 6:2af62394c832 25 volatile int counts2;
s1725696 6:2af62394c832 26 volatile double tower_1_position = 0.1; // the tower which he reaches first
s1725696 6:2af62394c832 27 volatile double tower_end_position = 0.1; // the tower which he reaches second
s1725696 6:2af62394c832 28 volatile double rotation_start_position = 0.1; // the position where the rotation will remain
s1725696 7:32caebeb5c3a 29 volatile double position;
s1725696 9:6672bbf14f7a 30 float speed = 0.70;
s1725696 9:6672bbf14f7a 31 int dir = 0;
kweisbeek 0:1045216da12e 32
s1725696 10:4d5af6e84c7a 33 // Functions
s1725696 7:32caebeb5c3a 34 int Counts1(volatile int& a) // a = counts1
s1725696 6:2af62394c832 35 {
s1725696 6:2af62394c832 36 counts1_prev = a;
s1725696 8:ebb645b3e15f 37 a = Encoder2.getPulses();
s1725696 7:32caebeb5c3a 38 return a;
s1725696 6:2af62394c832 39 }
s1725696 6:2af62394c832 40
s1725696 7:32caebeb5c3a 41 int Counts2(volatile int& a) // a = counts2
s1725696 6:2af62394c832 42 {
s1725696 6:2af62394c832 43 counts2_prev = a;
s1725696 8:ebb645b3e15f 44 a = Encoder1.getPulses();
s1725696 7:32caebeb5c3a 45 return a;
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 7:32caebeb5c3a 58 }
s1725696 6:2af62394c832 59 }
s1725696 6:2af62394c832 60
s1725696 6:2af62394c832 61 // Start translation
s1725696 6:2af62394c832 62 void translation_start(int a, float b) // a = dir , b = speed
s1725696 6:2af62394c832 63 {
s1725696 10:4d5af6e84c7a 64 dirpin_1.write(a);
s1725696 10:4d5af6e84c7a 65 pwmpin_1 = b;
s1725696 6:2af62394c832 66 }
s1725696 6:2af62394c832 67
s1725696 6:2af62394c832 68 // Stop translation
s1725696 6:2af62394c832 69 void translation_stop()
s1725696 6:2af62394c832 70 {
s1725696 10:4d5af6e84c7a 71 pwmpin_1 = 0.0;
s1725696 6:2af62394c832 72 }
s1725696 6:2af62394c832 73
s1725696 6:2af62394c832 74 // Start rotation
s1725696 7:32caebeb5c3a 75 void rotation_start(int a, float b)
s1725696 6:2af62394c832 76 {
s1725696 6:2af62394c832 77 dirpin_2.write(a);
s1725696 6:2af62394c832 78 pwmpin_2 = b;
s1725696 6:2af62394c832 79 }
s1725696 6:2af62394c832 80
s1725696 6:2af62394c832 81 // Stop rotation
s1725696 7:32caebeb5c3a 82 void rotation_stop()
s1725696 6:2af62394c832 83 {
s1725696 6:2af62394c832 84 pwmpin_2 = 0.0;
s1725696 6:2af62394c832 85 }
kweisbeek 1:53bb5928adcf 86
s1725696 9:6672bbf14f7a 87 // Calibration of translation
s1725696 9:6672bbf14f7a 88 void calibration_translation()
s1725696 6:2af62394c832 89 {
s1725696 7:32caebeb5c3a 90 for(int m = 1; m <= 2; m++) // to do each direction one time
s1725696 6:2af62394c832 91 {
s1725696 7:32caebeb5c3a 92 pc.printf("\r\nTranslatie loop\r\n");
s1725696 6:2af62394c832 93 translation_start(dir,speed);
s1725696 7:32caebeb5c3a 94 pc.printf("Direction = %i\r\n", dir);
s1725696 6:2af62394c832 95
s1725696 7:32caebeb5c3a 96 bool g = true; // to make a condition for the while loop
s1725696 6:2af62394c832 97 while (g == true)
s1725696 6:2af62394c832 98 {
s1725696 7:32caebeb5c3a 99 if (button_demo == false) // if button_demo is pushed, the translation should stop and change direction
s1725696 6:2af62394c832 100 {
s1725696 6:2af62394c832 101 translation_stop();
s1725696 8:ebb645b3e15f 102 pos_store(Counts1(counts1));
s1725696 7:32caebeb5c3a 103 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 104 dir = dir + 1;
s1725696 7:32caebeb5c3a 105
s1725696 6:2af62394c832 106 g = false; // to end the while loop
s1725696 6:2af62394c832 107 }
s1725696 7:32caebeb5c3a 108
s1725696 6:2af62394c832 109 wait(0.01);
s1725696 6:2af62394c832 110 }
s1725696 7:32caebeb5c3a 111
s1725696 7:32caebeb5c3a 112 wait(1.5); // wait 3 seconds before next round of translation/rotation
s1725696 9:6672bbf14f7a 113 }
s1725696 9:6672bbf14f7a 114 }
s1725696 7:32caebeb5c3a 115
s1725696 9:6672bbf14f7a 116 void calibration_rotation()
s1725696 9:6672bbf14f7a 117 {
s1725696 7:32caebeb5c3a 118 rotation_start(dir, speed);
s1725696 7:32caebeb5c3a 119 pc.printf("\r\nRotatie start\r\n");
s1725696 6:2af62394c832 120
s1725696 6:2af62394c832 121 bool f = true; // condition for while loop
s1725696 6:2af62394c832 122 while(f == true)
s1725696 6:2af62394c832 123 {
s1725696 7:32caebeb5c3a 124 if (button_motorcal == false) // If button_motorcal is pushed, then the motor should stop and remain in that position until homing
s1725696 6:2af62394c832 125 {
s1725696 7:32caebeb5c3a 126 rotation_stop();
s1725696 7:32caebeb5c3a 127
s1725696 6:2af62394c832 128 f = false; // to end the while loop
s1725696 6:2af62394c832 129 }
s1725696 7:32caebeb5c3a 130
s1725696 7:32caebeb5c3a 131 wait(0.01);
s1725696 7:32caebeb5c3a 132 }
s1725696 7:32caebeb5c3a 133
s1725696 7:32caebeb5c3a 134 pos_store(Counts2(counts2));
s1725696 7:32caebeb5c3a 135 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 9:6672bbf14f7a 136 }
s1725696 9:6672bbf14f7a 137
s1725696 9:6672bbf14f7a 138 void MOTOR_CAL()
s1725696 9:6672bbf14f7a 139 {
s1725696 9:6672bbf14f7a 140 // translation
s1725696 9:6672bbf14f7a 141 calibration_translation();
s1725696 9:6672bbf14f7a 142
s1725696 9:6672bbf14f7a 143 pc.printf("before wait\r\n");
s1725696 9:6672bbf14f7a 144 wait(3.0);
s1725696 9:6672bbf14f7a 145
s1725696 9:6672bbf14f7a 146 // rotation
s1725696 9:6672bbf14f7a 147 calibration_rotation();
s1725696 6:2af62394c832 148
s1725696 7:32caebeb5c3a 149 pc.printf("Motor calibration done");
s1725696 9:6672bbf14f7a 150 }
s1725696 7:32caebeb5c3a 151
s1725696 9:6672bbf14f7a 152
s1725696 9:6672bbf14f7a 153 // main function
s1725696 9:6672bbf14f7a 154 int main()
s1725696 9:6672bbf14f7a 155 {
s1725696 9:6672bbf14f7a 156 pc.baud(115200);
s1725696 9:6672bbf14f7a 157 pc.printf("Start\r\n");
s1725696 9:6672bbf14f7a 158 pwmpin.period_us(60);
s1725696 9:6672bbf14f7a 159
s1725696 9:6672bbf14f7a 160 MOTOR_CAL();
s1725696 9:6672bbf14f7a 161
s1725696 7:32caebeb5c3a 162 }