Chanaka Madhusanka / Mbed 2 deprecated Encoder_test

first test

Dependencies:   mbed QEI QEI_hw SDFileSystem

main.cpp@1:014671c2d6da, 2019-10-30 (annotated)

Committer:
chanaka_madhusanka
Date:
Wed Oct 30 06:57:25 2019 +0000
Revision:
1:014671c2d6da
Parent:
0:0205108c2c99 
30102019 current control manual

Who changed what in which revision?

UserRevisionLine numberNew contents of line
chanaka_madhusanka 0:0205108c2c99 1 #include "mbed.h"
chanaka_madhusanka 1:014671c2d6da 2 #include "SDFileSystem.h"
chanaka_madhusanka 1:014671c2d6da 3 #include "QEI.h"
chanaka_madhusanka 1:014671c2d6da 4
chanaka_madhusanka 1:014671c2d6da 5 SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board p5 - mosi, p6-miso, p7 - sck, p8 - cs
chanaka_madhusanka 1:014671c2d6da 6 Serial pc (USBTX,USBRX);
chanaka_madhusanka 1:014671c2d6da 7 QEI wheel (p29, p30, NC, 2000, QEI::X4_ENCODING); //to get encoder position
chanaka_madhusanka 1:014671c2d6da 8
chanaka_madhusanka 1:014671c2d6da 9 //int i= 2;
chanaka_madhusanka 1:014671c2d6da 10 int value = 0;
chanaka_madhusanka 1:014671c2d6da 11
chanaka_madhusanka 1:014671c2d6da 12 void sendDAC(int value);
chanaka_madhusanka 1:014671c2d6da 13 void file_write_int(double input, double torque);
chanaka_madhusanka 1:014671c2d6da 14
chanaka_madhusanka 0:0205108c2c99 15
chanaka_madhusanka 1:014671c2d6da 16 //DAC 712
chanaka_madhusanka 1:014671c2d6da 17 DigitalOut dac_a0(p13);
chanaka_madhusanka 1:014671c2d6da 18 DigitalOut dac_a1(p12);
chanaka_madhusanka 1:014671c2d6da 19 DigitalOut dac_wr(p9);
chanaka_madhusanka 1:014671c2d6da 20 DigitalOut dac_clr(p10);
chanaka_madhusanka 1:014671c2d6da 21 DigitalOut dac_d15(p15);
chanaka_madhusanka 1:014671c2d6da 22 DigitalOut dac_d14(p16);
chanaka_madhusanka 1:014671c2d6da 23 DigitalOut dac_d13(p17);
chanaka_madhusanka 1:014671c2d6da 24 DigitalOut dac_d12(p18);
chanaka_madhusanka 1:014671c2d6da 25 DigitalOut dac_d11(p19);
chanaka_madhusanka 1:014671c2d6da 26 DigitalOut dac_d10(p20);
chanaka_madhusanka 1:014671c2d6da 27 DigitalOut dac_d9(p21);
chanaka_madhusanka 1:014671c2d6da 28 DigitalOut dac_d8(p22);
chanaka_madhusanka 1:014671c2d6da 29 DigitalOut dac_d7(p23);
chanaka_madhusanka 1:014671c2d6da 30 DigitalOut dac_d6(p24);
chanaka_madhusanka 1:014671c2d6da 31 DigitalOut dac_d5(p25);
chanaka_madhusanka 1:014671c2d6da 32 DigitalOut dac_d4(p26);
chanaka_madhusanka 1:014671c2d6da 33 DigitalOut dac_d3(p27);
chanaka_madhusanka 1:014671c2d6da 34 DigitalOut dac_d2(p28);
chanaka_madhusanka 1:014671c2d6da 35 DigitalOut dac_d1(p11); //p29 for writing purpose change p29 to p11
chanaka_madhusanka 1:014671c2d6da 36 DigitalOut dac_d0(p14); //p30 for writing purpose change p30 to p14
chanaka_madhusanka 0:0205108c2c99 37
chanaka_madhusanka 1:014671c2d6da 38 int main(){
chanaka_madhusanka 1:014671c2d6da 39
chanaka_madhusanka 1:014671c2d6da 40 float val = 32767/10;
chanaka_madhusanka 1:014671c2d6da 41 int sendval;
chanaka_madhusanka 1:014671c2d6da 42 double position;
chanaka_madhusanka 1:014671c2d6da 43 double x;
chanaka_madhusanka 1:014671c2d6da 44
chanaka_madhusanka 1:014671c2d6da 45 while(1){
chanaka_madhusanka 1:014671c2d6da 46 //reverse loop
chanaka_madhusanka 1:014671c2d6da 47 dac_a0 = 0;
chanaka_madhusanka 1:014671c2d6da 48 dac_a1 = 1;
chanaka_madhusanka 1:014671c2d6da 49 dac_clr = 1;
chanaka_madhusanka 1:014671c2d6da 50 sendval = int(val * 0.06*(0.9)); //for maximum torque = 46.75 voltage = 2.805 >>> 0 is not zero. Eventhough "0.9" is a positive it gives negative direction movement for "3" it gives positive movement
chanaka_madhusanka 1:014671c2d6da 51 sendDAC(sendval);
chanaka_madhusanka 1:014671c2d6da 52 dac_a0 = 1;
chanaka_madhusanka 1:014671c2d6da 53 dac_a1 = 0;
chanaka_madhusanka 1:014671c2d6da 54 wait(0.1); //to change the back velocity
chanaka_madhusanka 1:014671c2d6da 55
chanaka_madhusanka 1:014671c2d6da 56 dac_a0 = 0;
chanaka_madhusanka 1:014671c2d6da 57 dac_a1 = 1;
chanaka_madhusanka 1:014671c2d6da 58 dac_clr = 1;
chanaka_madhusanka 1:014671c2d6da 59 sendval = int(val * 0.06*(1.4)); //for maximum torque = 46.75 voltage = 2.805 >>> 0 is not zero. Eventhough "0.9" is a positive it gives negative direction movement for "3" it gives positive movement
chanaka_madhusanka 1:014671c2d6da 60 sendDAC(sendval);
chanaka_madhusanka 1:014671c2d6da 61 dac_a0 = 1;
chanaka_madhusanka 1:014671c2d6da 62 dac_a1 = 0;
chanaka_madhusanka 1:014671c2d6da 63 wait(3); //to change the back velocity
chanaka_madhusanka 1:014671c2d6da 64
chanaka_madhusanka 1:014671c2d6da 65 //forward loop
chanaka_madhusanka 1:014671c2d6da 66 dac_a0 = 0;
chanaka_madhusanka 1:014671c2d6da 67 dac_a1 = 1;
chanaka_madhusanka 1:014671c2d6da 68 sendval = int(val * 0.06*(6));
chanaka_madhusanka 1:014671c2d6da 69 sendDAC(sendval);
chanaka_madhusanka 1:014671c2d6da 70 dac_a0 = 1;
chanaka_madhusanka 1:014671c2d6da 71 dac_a1 = 0;
chanaka_madhusanka 1:014671c2d6da 72 wait(4);
chanaka_madhusanka 1:014671c2d6da 73
chanaka_madhusanka 1:014671c2d6da 74 position = 0;
chanaka_madhusanka 1:014671c2d6da 75 x = -wheel.getPulses()/200.0;
chanaka_madhusanka 1:014671c2d6da 76
chanaka_madhusanka 1:014671c2d6da 77 //for increasing torque
chanaka_madhusanka 1:014671c2d6da 78 for(double i = 1 ; i<=20 ; i+=0.001){
chanaka_madhusanka 1:014671c2d6da 79
chanaka_madhusanka 1:014671c2d6da 80 double torque;
chanaka_madhusanka 1:014671c2d6da 81
chanaka_madhusanka 1:014671c2d6da 82 if(i<10.1){
chanaka_madhusanka 1:014671c2d6da 83 dac_a0 = 0;
chanaka_madhusanka 1:014671c2d6da 84 dac_a1 = 1;
chanaka_madhusanka 1:014671c2d6da 85 sendval = int(val * (0.0284*i+0.1436));
chanaka_madhusanka 1:014671c2d6da 86 sendDAC(sendval);
chanaka_madhusanka 1:014671c2d6da 87 dac_a0 = 1;
chanaka_madhusanka 1:014671c2d6da 88 dac_a1 = 0;
chanaka_madhusanka 1:014671c2d6da 89 torque = 25*0.6*0.4*(0.0284*i+0.1436)/1.2796; //maximum percentage is 40... range 0-40
chanaka_madhusanka 1:014671c2d6da 90 position = (-wheel.getPulses()/200.0)-x;
chanaka_madhusanka 1:014671c2d6da 91
chanaka_madhusanka 1:014671c2d6da 92 if((wheel.getPulses()/200.0)<6){
chanaka_madhusanka 1:014671c2d6da 93 file_write_int(position,torque);
chanaka_madhusanka 1:014671c2d6da 94 }
chanaka_madhusanka 1:014671c2d6da 95 pc.printf("Pulses is: %lf\t %lf\r\n",position,torque);
chanaka_madhusanka 1:014671c2d6da 96
chanaka_madhusanka 1:014671c2d6da 97 if(i>9.9){
chanaka_madhusanka 1:014671c2d6da 98 for(int j=1; j<500 ;j++){
chanaka_madhusanka 1:014671c2d6da 99 pc.printf("Wait position : %lf\t %lf\r\n",-(wheel.getPulses()/200.0)-x,torque);
chanaka_madhusanka 1:014671c2d6da 100 }
chanaka_madhusanka 1:014671c2d6da 101 }
chanaka_madhusanka 1:014671c2d6da 102 }else if(i>=10.1){
chanaka_madhusanka 1:014671c2d6da 103
chanaka_madhusanka 1:014671c2d6da 104 dac_a0 = 0;
chanaka_madhusanka 1:014671c2d6da 105 dac_a1 = 1;
chanaka_madhusanka 1:014671c2d6da 106 sendval = int(val * (0.0284*i+0.1436));
chanaka_madhusanka 1:014671c2d6da 107 sendDAC(sendval);
chanaka_madhusanka 1:014671c2d6da 108 dac_a0 = 1;
chanaka_madhusanka 1:014671c2d6da 109 dac_a1 = 0;
chanaka_madhusanka 1:014671c2d6da 110 torque = 25*0.6*0.4*(6+20-i)/2.805;
chanaka_madhusanka 1:014671c2d6da 111 position = (-wheel.getPulses()/200.0)-x;
chanaka_madhusanka 1:014671c2d6da 112
chanaka_madhusanka 1:014671c2d6da 113 if((wheel.getPulses()/200.0)<6){
chanaka_madhusanka 1:014671c2d6da 114 file_write_int(position,torque);
chanaka_madhusanka 1:014671c2d6da 115 }
chanaka_madhusanka 1:014671c2d6da 116 pc.printf("Pulses is: %lf\t %lf\r\n",position,torque);
chanaka_madhusanka 1:014671c2d6da 117 }
chanaka_madhusanka 1:014671c2d6da 118 wait(0.05);
chanaka_madhusanka 1:014671c2d6da 119 }
chanaka_madhusanka 1:014671c2d6da 120 }
chanaka_madhusanka 0:0205108c2c99 121 }
chanaka_madhusanka 0:0205108c2c99 122
chanaka_madhusanka 1:014671c2d6da 123 void sendDAC(int value){
chanaka_madhusanka 1:014671c2d6da 124 int remainder = 0;
chanaka_madhusanka 1:014671c2d6da 125 dac_clr = 0;
chanaka_madhusanka 1:014671c2d6da 126 wait_us(120);
chanaka_madhusanka 1:014671c2d6da 127 dac_clr = 1;
chanaka_madhusanka 1:014671c2d6da 128 //send output dac
chanaka_madhusanka 1:014671c2d6da 129 dac_wr = 1;
chanaka_madhusanka 1:014671c2d6da 130 if(value >= 0){
chanaka_madhusanka 1:014671c2d6da 131 if(value >= 32767){
chanaka_madhusanka 1:014671c2d6da 132 dac_d15 = 0; dac_d14= 1; dac_d13 = 1; dac_d12 = 1; dac_d11 = 1; dac_d10 =1; dac_d9 = 1; dac_d8 = 1; dac_d7 = 1; dac_d6 = 1; dac_d5 = 1; dac_d4 = 1; dac_d3 = 1; dac_d2 = 1; dac_d1 =1; dac_d0 = 1;
chanaka_madhusanka 1:014671c2d6da 133 }
chanaka_madhusanka 1:014671c2d6da 134 else {
chanaka_madhusanka 1:014671c2d6da 135 dac_d15 = 0;
chanaka_madhusanka 1:014671c2d6da 136 dac_d14 = value / 16384 ;
chanaka_madhusanka 1:014671c2d6da 137 remainder = value - dac_d14.read() * 16384;
chanaka_madhusanka 1:014671c2d6da 138 dac_d13 = remainder / 8192;
chanaka_madhusanka 1:014671c2d6da 139 remainder = remainder - dac_d13.read() * 8192;
chanaka_madhusanka 1:014671c2d6da 140 dac_d12 = remainder / 4096;
chanaka_madhusanka 1:014671c2d6da 141 remainder = remainder - dac_d12.read() * 4096;
chanaka_madhusanka 1:014671c2d6da 142 dac_d11 = remainder / 2048;
chanaka_madhusanka 1:014671c2d6da 143 remainder = remainder - dac_d11.read() * 2048;
chanaka_madhusanka 1:014671c2d6da 144 dac_d10 = remainder / 1024;
chanaka_madhusanka 1:014671c2d6da 145 remainder = remainder - dac_d10.read() * 1024;
chanaka_madhusanka 1:014671c2d6da 146 dac_d9 = remainder / 512;
chanaka_madhusanka 1:014671c2d6da 147 remainder = remainder - dac_d9.read() * 512;
chanaka_madhusanka 1:014671c2d6da 148 dac_d8 = remainder / 256;
chanaka_madhusanka 1:014671c2d6da 149 remainder = remainder - dac_d8.read() * 256;
chanaka_madhusanka 1:014671c2d6da 150 dac_d7 = remainder / 128;
chanaka_madhusanka 1:014671c2d6da 151 remainder = remainder - dac_d7.read() * 128;
chanaka_madhusanka 1:014671c2d6da 152 dac_d6 = remainder / 64;
chanaka_madhusanka 1:014671c2d6da 153 remainder = remainder - dac_d6.read() * 64;
chanaka_madhusanka 1:014671c2d6da 154 dac_d5 = remainder / 32;
chanaka_madhusanka 1:014671c2d6da 155 remainder = remainder - dac_d5.read() * 32;
chanaka_madhusanka 1:014671c2d6da 156 dac_d4 = remainder / 16;
chanaka_madhusanka 1:014671c2d6da 157 remainder = remainder - dac_d4.read() * 16;
chanaka_madhusanka 1:014671c2d6da 158 dac_d3 = remainder / 8;
chanaka_madhusanka 1:014671c2d6da 159 remainder = remainder - dac_d3.read() * 8;
chanaka_madhusanka 1:014671c2d6da 160 dac_d2 = remainder / 4;
chanaka_madhusanka 1:014671c2d6da 161 remainder = remainder - dac_d2.read() * 4;
chanaka_madhusanka 1:014671c2d6da 162 dac_d1 = remainder / 2;
chanaka_madhusanka 1:014671c2d6da 163 remainder = remainder - dac_d1.read() * 2;
chanaka_madhusanka 1:014671c2d6da 164 dac_d0 = remainder ;
chanaka_madhusanka 1:014671c2d6da 165 }
chanaka_madhusanka 0:0205108c2c99 166 }
chanaka_madhusanka 1:014671c2d6da 167 if (value < 0){
chanaka_madhusanka 1:014671c2d6da 168 if(value <= -32768){
chanaka_madhusanka 1:014671c2d6da 169 dac_d15 = 1; dac_d14= 0; dac_d13 = 0; dac_d12 = 0; dac_d11 = 0; dac_d10 =0; dac_d9 = 0; dac_d8 = 0; dac_d7 = 0; dac_d6 = 0; dac_d5 = 0; dac_d4 = 0; dac_d3 = 0; dac_d2 = 0; dac_d1 =0; dac_d0 = 0;
chanaka_madhusanka 1:014671c2d6da 170 }
chanaka_madhusanka 1:014671c2d6da 171 else{
chanaka_madhusanka 1:014671c2d6da 172 dac_d15 = 1;
chanaka_madhusanka 1:014671c2d6da 173 dac_d14 = ( 32768 + value) / 16384 ;
chanaka_madhusanka 1:014671c2d6da 174 remainder = ( 32768 + value) - dac_d14.read() * 16384;
chanaka_madhusanka 1:014671c2d6da 175 dac_d13 = remainder / 8192;
chanaka_madhusanka 1:014671c2d6da 176 remainder = remainder - dac_d13.read() * 8192;
chanaka_madhusanka 1:014671c2d6da 177 dac_d12 = remainder / 4096;
chanaka_madhusanka 1:014671c2d6da 178 remainder = remainder - dac_d12.read() * 4096;
chanaka_madhusanka 1:014671c2d6da 179 dac_d11 = remainder / 2048;
chanaka_madhusanka 1:014671c2d6da 180 remainder = remainder - dac_d11.read() * 2048;
chanaka_madhusanka 1:014671c2d6da 181 dac_d10 = remainder / 1024;
chanaka_madhusanka 1:014671c2d6da 182 remainder = remainder - dac_d10.read() * 1024;
chanaka_madhusanka 1:014671c2d6da 183 dac_d9 = remainder / 512;
chanaka_madhusanka 1:014671c2d6da 184 remainder = remainder - dac_d9.read() * 512;
chanaka_madhusanka 1:014671c2d6da 185 dac_d8 = remainder / 256;
chanaka_madhusanka 1:014671c2d6da 186 remainder = remainder - dac_d8.read() * 256;
chanaka_madhusanka 1:014671c2d6da 187 dac_d7 = remainder / 128;
chanaka_madhusanka 1:014671c2d6da 188 remainder = remainder - dac_d7.read() * 128;
chanaka_madhusanka 1:014671c2d6da 189 dac_d6 = remainder / 64;
chanaka_madhusanka 1:014671c2d6da 190 remainder = remainder - dac_d6.read() * 64;
chanaka_madhusanka 1:014671c2d6da 191 dac_d5 = remainder / 32;
chanaka_madhusanka 1:014671c2d6da 192 remainder = remainder - dac_d5.read() * 32;
chanaka_madhusanka 1:014671c2d6da 193 dac_d4 = remainder / 16;
chanaka_madhusanka 1:014671c2d6da 194 remainder = remainder - dac_d4.read() * 16;
chanaka_madhusanka 1:014671c2d6da 195 dac_d3 = remainder / 8;
chanaka_madhusanka 1:014671c2d6da 196 remainder = remainder - dac_d3.read() * 8;
chanaka_madhusanka 1:014671c2d6da 197 dac_d2 = remainder / 4;
chanaka_madhusanka 1:014671c2d6da 198 remainder = remainder - dac_d2.read() * 4;
chanaka_madhusanka 1:014671c2d6da 199 dac_d1 = remainder / 2 ;
chanaka_madhusanka 1:014671c2d6da 200 remainder = remainder - dac_d1.read() * 2;
chanaka_madhusanka 1:014671c2d6da 201 dac_d0 = remainder ;
chanaka_madhusanka 1:014671c2d6da 202 }
chanaka_madhusanka 0:0205108c2c99 203 }
chanaka_madhusanka 1:014671c2d6da 204 dac_wr = 0;
chanaka_madhusanka 1:014671c2d6da 205 }
chanaka_madhusanka 1:014671c2d6da 206
chanaka_madhusanka 1:014671c2d6da 207 void file_write_int(double position,double torque){
chanaka_madhusanka 1:014671c2d6da 208 mkdir("/sd/mydir", 0777);
chanaka_madhusanka 1:014671c2d6da 209 FILE *fp = fopen("/sd/mydir/sdtesthys.txt", "a");
chanaka_madhusanka 1:014671c2d6da 210 if(fp == NULL){
chanaka_madhusanka 1:014671c2d6da 211 error("Could not open file for write\n");
chanaka_madhusanka 0:0205108c2c99 212 }
chanaka_madhusanka 1:014671c2d6da 213 fprintf(fp,"%lf\t %lf\t \r\n",position,torque);
chanaka_madhusanka 1:014671c2d6da 214 fclose(fp);
chanaka_madhusanka 0:0205108c2c99 215 }
chanaka_madhusanka 1:014671c2d6da 216
chanaka_madhusanka 1:014671c2d6da 217