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?
User | Revision | Line number | New 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 |