USNA ES305 / Mbed 2 deprecated Lab9

None

Dependencies:   mbed

main.cpp@0:1de151a3fae9, 2014-11-20 (annotated)

Committer:
riobrien
Date:
Thu Nov 20 14:48:44 2014 +0000
Revision:
0:1de151a3fae9
None

Who changed what in which revision?

UserRevisionLine numberNew contents of line
riobrien 0:1de151a3fae9 1 #include "mbed.h"
riobrien 0:1de151a3fae9 2 #define PI (3.14159)
riobrien 0:1de151a3fae9 3 //=============================================================================
riobrien 0:1de151a3fae9 4 // Four commands for the Instruction Register (B7,B6) - LS7366
riobrien 0:1de151a3fae9 5 //=============================================================================
riobrien 0:1de151a3fae9 6 #define CLR 0x00 //Clear Instruction
riobrien 0:1de151a3fae9 7 #define RD 0x01 //Read Instruction
riobrien 0:1de151a3fae9 8 #define WR 0x02 //Write Instruction
riobrien 0:1de151a3fae9 9 #define LOAD 0x03 //Load Instruction
riobrien 0:1de151a3fae9 10
riobrien 0:1de151a3fae9 11 //=============================================================================
riobrien 0:1de151a3fae9 12 // Register to Select from the Instruction Register (B5,B4,B3) - LS7366
riobrien 0:1de151a3fae9 13 //=============================================================================
riobrien 0:1de151a3fae9 14 #define NONE 0x00 //No Register Selected
riobrien 0:1de151a3fae9 15 #define MDR0 0x01 //Mode Register 0
riobrien 0:1de151a3fae9 16 #define MDR1 0x02 //Mode Register 1
riobrien 0:1de151a3fae9 17 #define DTR 0x03 //Data Transfer Register
riobrien 0:1de151a3fae9 18 #define CNTR 0x04 //Software Configurable Counter Register
riobrien 0:1de151a3fae9 19 #define OTR 0x05 //Output Transfer Register
riobrien 0:1de151a3fae9 20 #define STR 0x06 //Status Register
riobrien 0:1de151a3fae9 21 #define NONE_REG 0x07 //No Register Selected
riobrien 0:1de151a3fae9 22
riobrien 0:1de151a3fae9 23
riobrien 0:1de151a3fae9 24 Serial pc(USBTX,USBRX);
riobrien 0:1de151a3fae9 25 SPI spi(p5, p6, p7);
riobrien 0:1de151a3fae9 26 LocalFileSystem local ("local");
riobrien 0:1de151a3fae9 27 DigitalOut ls7166_cs1(p19); //CS for LS7366
riobrien 0:1de151a3fae9 28 DigitalOut ls7166_cs2(p20); //CS for LS7366
riobrien 0:1de151a3fae9 29 DigitalOut mot1_ph1(p21);
riobrien 0:1de151a3fae9 30 PwmOut mot_en1(p23);
riobrien 0:1de151a3fae9 31 //----- LS7366 Encoder/Counter Routines --------------------
riobrien 0:1de151a3fae9 32 void LS7366_cmd(int inst, int reg){
riobrien 0:1de151a3fae9 33 char cmd;
riobrien 0:1de151a3fae9 34
riobrien 0:1de151a3fae9 35 spi.format(8, 0);
riobrien 0:1de151a3fae9 36 spi.frequency(2000000);
riobrien 0:1de151a3fae9 37 cmd = (inst << 6) | (reg << 3);
riobrien 0:1de151a3fae9 38 // printf("\r\ncmd=0X%2X", cmd);
riobrien 0:1de151a3fae9 39 spi.write(cmd);
riobrien 0:1de151a3fae9 40 }
riobrien 0:1de151a3fae9 41
riobrien 0:1de151a3fae9 42 long LS7366_read_counter(int chan_num){
riobrien 0:1de151a3fae9 43 union bytes{
riobrien 0:1de151a3fae9 44 char byte_enc[4];
riobrien 0:1de151a3fae9 45 long long_enc;
riobrien 0:1de151a3fae9 46 }counter;
riobrien 0:1de151a3fae9 47
riobrien 0:1de151a3fae9 48 spi.format(8, 0);
riobrien 0:1de151a3fae9 49 spi.frequency(2000000);
riobrien 0:1de151a3fae9 50
riobrien 0:1de151a3fae9 51 if(chan_num!=2){
riobrien 0:1de151a3fae9 52 ls7166_cs1 = 0;
riobrien 0:1de151a3fae9 53 wait_us(1);
riobrien 0:1de151a3fae9 54 LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR
riobrien 0:1de151a3fae9 55 ls7166_cs1 = 1;
riobrien 0:1de151a3fae9 56 wait_us(1);
riobrien 0:1de151a3fae9 57 ls7166_cs1 = 0;
riobrien 0:1de151a3fae9 58 }
riobrien 0:1de151a3fae9 59 else{
riobrien 0:1de151a3fae9 60 ls7166_cs2 = 0;
riobrien 0:1de151a3fae9 61 wait_us(1);
riobrien 0:1de151a3fae9 62 LS7366_cmd(LOAD,OTR);//cmd = 0xe8, LOAD to OTR
riobrien 0:1de151a3fae9 63 ls7166_cs2 = 1;
riobrien 0:1de151a3fae9 64 wait_us(1);
riobrien 0:1de151a3fae9 65
riobrien 0:1de151a3fae9 66 ls7166_cs2 = 0;
riobrien 0:1de151a3fae9 67 }
riobrien 0:1de151a3fae9 68 wait_us(1);
riobrien 0:1de151a3fae9 69 LS7366_cmd(RD,CNTR); //cmd = 0x60, READ from CNTR
riobrien 0:1de151a3fae9 70 counter.byte_enc[3] = spi.write(0x00);
riobrien 0:1de151a3fae9 71 counter.byte_enc[2] = spi.write(0x00);
riobrien 0:1de151a3fae9 72 counter.byte_enc[1] = spi.write(0x00);
riobrien 0:1de151a3fae9 73 counter.byte_enc[0] = spi.write(0x00);
riobrien 0:1de151a3fae9 74
riobrien 0:1de151a3fae9 75 if(chan_num!=2){
riobrien 0:1de151a3fae9 76 ls7166_cs1 = 1;
riobrien 0:1de151a3fae9 77 }
riobrien 0:1de151a3fae9 78 else{
riobrien 0:1de151a3fae9 79 ls7166_cs2 = 1;
riobrien 0:1de151a3fae9 80 }
riobrien 0:1de151a3fae9 81
riobrien 0:1de151a3fae9 82 return counter.long_enc; //return count
riobrien 0:1de151a3fae9 83 }
riobrien 0:1de151a3fae9 84
riobrien 0:1de151a3fae9 85 void LS7366_quad_mode_x4(int chan_num){
riobrien 0:1de151a3fae9 86
riobrien 0:1de151a3fae9 87 spi.format(8, 0);
riobrien 0:1de151a3fae9 88 spi.frequency(2000000);
riobrien 0:1de151a3fae9 89
riobrien 0:1de151a3fae9 90 if(chan_num!=2){
riobrien 0:1de151a3fae9 91 ls7166_cs1 = 0;
riobrien 0:1de151a3fae9 92 }
riobrien 0:1de151a3fae9 93 else{
riobrien 0:1de151a3fae9 94 ls7166_cs2 = 0;
riobrien 0:1de151a3fae9 95 }
riobrien 0:1de151a3fae9 96 wait_us(1);
riobrien 0:1de151a3fae9 97 LS7366_cmd(WR,MDR0);// Write to the MDR0 register
riobrien 0:1de151a3fae9 98 spi.write(0x03); // X4 quadrature count mode
riobrien 0:1de151a3fae9 99 if(chan_num!=2){
riobrien 0:1de151a3fae9 100 ls7166_cs1 = 1;
riobrien 0:1de151a3fae9 101 }
riobrien 0:1de151a3fae9 102 else{
riobrien 0:1de151a3fae9 103 ls7166_cs2 = 1;
riobrien 0:1de151a3fae9 104 }
riobrien 0:1de151a3fae9 105 }
riobrien 0:1de151a3fae9 106
riobrien 0:1de151a3fae9 107 void LS7366_reset_counter(int chan_num){
riobrien 0:1de151a3fae9 108
riobrien 0:1de151a3fae9 109 spi.format(8, 0);
riobrien 0:1de151a3fae9 110 spi.frequency(2000000);
riobrien 0:1de151a3fae9 111
riobrien 0:1de151a3fae9 112 if(chan_num!=2){
riobrien 0:1de151a3fae9 113 ls7166_cs1 = 0;
riobrien 0:1de151a3fae9 114 }
riobrien 0:1de151a3fae9 115 else{
riobrien 0:1de151a3fae9 116 ls7166_cs2 = 0;
riobrien 0:1de151a3fae9 117 }
riobrien 0:1de151a3fae9 118 wait_us(1);
riobrien 0:1de151a3fae9 119 LS7366_cmd(CLR,CNTR);//Clear the counter register
riobrien 0:1de151a3fae9 120 if(chan_num!=2){
riobrien 0:1de151a3fae9 121 ls7166_cs1 = 1;
riobrien 0:1de151a3fae9 122 }
riobrien 0:1de151a3fae9 123 else{
riobrien 0:1de151a3fae9 124 ls7166_cs2 = 1;
riobrien 0:1de151a3fae9 125 }
riobrien 0:1de151a3fae9 126 wait_us(1);
riobrien 0:1de151a3fae9 127
riobrien 0:1de151a3fae9 128 if(chan_num!=2){
riobrien 0:1de151a3fae9 129 ls7166_cs1 = 0;
riobrien 0:1de151a3fae9 130 }
riobrien 0:1de151a3fae9 131 else{
riobrien 0:1de151a3fae9 132 ls7166_cs2 = 0;
riobrien 0:1de151a3fae9 133 }
riobrien 0:1de151a3fae9 134 wait_us(1);
riobrien 0:1de151a3fae9 135 LS7366_cmd(LOAD,CNTR);//
riobrien 0:1de151a3fae9 136 if(chan_num!=2){
riobrien 0:1de151a3fae9 137 ls7166_cs1 = 1;
riobrien 0:1de151a3fae9 138 }
riobrien 0:1de151a3fae9 139 else{
riobrien 0:1de151a3fae9 140 ls7166_cs1 = 1;
riobrien 0:1de151a3fae9 141 }
riobrien 0:1de151a3fae9 142 }
riobrien 0:1de151a3fae9 143
riobrien 0:1de151a3fae9 144 void LS7366_write_DTR(int chan_num,long enc_value)
riobrien 0:1de151a3fae9 145 {
riobrien 0:1de151a3fae9 146 union bytes
riobrien 0:1de151a3fae9 147 {
riobrien 0:1de151a3fae9 148 char byte_enc[4];
riobrien 0:1de151a3fae9 149 long long_enc;
riobrien 0:1de151a3fae9 150 }counter;
riobrien 0:1de151a3fae9 151
riobrien 0:1de151a3fae9 152 spi.format(8, 0);
riobrien 0:1de151a3fae9 153 spi.frequency(2000000);
riobrien 0:1de151a3fae9 154
riobrien 0:1de151a3fae9 155 counter.long_enc = enc_value;
riobrien 0:1de151a3fae9 156
riobrien 0:1de151a3fae9 157 if(chan_num!=2){
riobrien 0:1de151a3fae9 158 ls7166_cs1 = 0;
riobrien 0:1de151a3fae9 159 }
riobrien 0:1de151a3fae9 160 else{
riobrien 0:1de151a3fae9 161 ls7166_cs2 = 0;
riobrien 0:1de151a3fae9 162 }
riobrien 0:1de151a3fae9 163 wait_us(1);
riobrien 0:1de151a3fae9 164 LS7366_cmd(WR,DTR);//
riobrien 0:1de151a3fae9 165 spi.write(counter.byte_enc[3]);
riobrien 0:1de151a3fae9 166 spi.write(counter.byte_enc[2]);
riobrien 0:1de151a3fae9 167 spi.write(counter.byte_enc[1]);
riobrien 0:1de151a3fae9 168 spi.write(counter.byte_enc[0]);
riobrien 0:1de151a3fae9 169 if(chan_num!=2){
riobrien 0:1de151a3fae9 170 ls7166_cs1 = 1;
riobrien 0:1de151a3fae9 171 }
riobrien 0:1de151a3fae9 172 else{
riobrien 0:1de151a3fae9 173 ls7166_cs2 = 1;
riobrien 0:1de151a3fae9 174 }
riobrien 0:1de151a3fae9 175
riobrien 0:1de151a3fae9 176 wait_us(1);
riobrien 0:1de151a3fae9 177 if(chan_num!=2){
riobrien 0:1de151a3fae9 178 ls7166_cs1 = 0;
riobrien 0:1de151a3fae9 179 }
riobrien 0:1de151a3fae9 180 else{
riobrien 0:1de151a3fae9 181 ls7166_cs2 = 0;
riobrien 0:1de151a3fae9 182 }
riobrien 0:1de151a3fae9 183 wait_us(1);
riobrien 0:1de151a3fae9 184 LS7366_cmd(LOAD,CNTR);//
riobrien 0:1de151a3fae9 185 if(chan_num!=2){
riobrien 0:1de151a3fae9 186 ls7166_cs1 = 1;
riobrien 0:1de151a3fae9 187 }
riobrien 0:1de151a3fae9 188 else{
riobrien 0:1de151a3fae9 189 ls7166_cs2 = 1;
riobrien 0:1de151a3fae9 190 }
riobrien 0:1de151a3fae9 191 }
riobrien 0:1de151a3fae9 192 float Ts = 0.001; // Sampling period 1/Ts Hz
riobrien 0:1de151a3fae9 193
riobrien 0:1de151a3fae9 194 // Arrays for data storage
riobrien 0:1de151a3fae9 195 float etime[1000];
riobrien 0:1de151a3fae9 196 float ang_pos[1000];
riobrien 0:1de151a3fae9 197 float est_speed[1000];
riobrien 0:1de151a3fae9 198 float dc_in[1000];
riobrien 0:1de151a3fae9 199 //float ang_posID[1100];
riobrien 0:1de151a3fae9 200 Timer t;
riobrien 0:1de151a3fae9 201
riobrien 0:1de151a3fae9 202 // Open "results.M" on the local file system for writing
riobrien 0:1de151a3fae9 203 FILE *fp = fopen("/local/Lab9a.M", "w");
riobrien 0:1de151a3fae9 204
riobrien 0:1de151a3fae9 205 float cntr;
riobrien 0:1de151a3fae9 206 float ang,angp,speed;
riobrien 0:1de151a3fae9 207 float dt;
riobrien 0:1de151a3fae9 208 float dc,dc_prev,err,err_prev;
riobrien 0:1de151a3fae9 209 float Kpi,zpi;
riobrien 0:1de151a3fae9 210 long enc1;
riobrien 0:1de151a3fae9 211 float K,dspeed;
riobrien 0:1de151a3fae9 212 int k,k0;
riobrien 0:1de151a3fae9 213
riobrien 0:1de151a3fae9 214 int main ()
riobrien 0:1de151a3fae9 215 {
riobrien 0:1de151a3fae9 216 pc.baud(921600); //Set up serial port baud rate
riobrien 0:1de151a3fae9 217 spi.frequency(5000000);
riobrien 0:1de151a3fae9 218 LS7366_reset_counter(1);
riobrien 0:1de151a3fae9 219 LS7366_quad_mode_x4(1);
riobrien 0:1de151a3fae9 220 LS7366_write_DTR(1,0);
riobrien 0:1de151a3fae9 221
riobrien 0:1de151a3fae9 222 LS7366_reset_counter(2);
riobrien 0:1de151a3fae9 223 LS7366_quad_mode_x4(2);
riobrien 0:1de151a3fae9 224 LS7366_write_DTR(2,0);
riobrien 0:1de151a3fae9 225 cntr = 0.0; // cntr used to keep track of sample period and elpased time
riobrien 0:1de151a3fae9 226 K = 0.00887;
riobrien 0:1de151a3fae9 227 Kpi = 0.011392;
riobrien 0:1de151a3fae9 228 zpi = 37.4;
riobrien 0:1de151a3fae9 229 dspeed = 40; // rad/sec
riobrien 0:1de151a3fae9 230 // initialize data vectors
riobrien 0:1de151a3fae9 231 for(k=0;k<1000;k++)
riobrien 0:1de151a3fae9 232 { etime[k] = 0.0;
riobrien 0:1de151a3fae9 233 ang_pos[k] = 0.0;
riobrien 0:1de151a3fae9 234 est_speed[k] = 0.0;
riobrien 0:1de151a3fae9 235 dc_in[k] = 0.0;
riobrien 0:1de151a3fae9 236 }
riobrien 0:1de151a3fae9 237 k = 0; // Reset index counter
riobrien 0:1de151a3fae9 238 k0 = 0;
riobrien 0:1de151a3fae9 239 angp = 0; // initialize previous angle variable
riobrien 0:1de151a3fae9 240 dc_prev = 0;
riobrien 0:1de151a3fae9 241 err_prev = 0;
riobrien 0:1de151a3fae9 242
riobrien 0:1de151a3fae9 243
riobrien 0:1de151a3fae9 244 while(cntr <= 1000) {
riobrien 0:1de151a3fae9 245 t.start(); // start measuring comp time
riobrien 0:1de151a3fae9 246
riobrien 0:1de151a3fae9 247 // Read encoder
riobrien 0:1de151a3fae9 248 enc1 = LS7366_read_counter(1);
riobrien 0:1de151a3fae9 249
riobrien 0:1de151a3fae9 250 // Convert from counts to radians
riobrien 0:1de151a3fae9 251 ang = -2*PI*enc1/6500.0;
riobrien 0:1de151a3fae9 252
riobrien 0:1de151a3fae9 253 // Estimate speed
riobrien 0:1de151a3fae9 254 speed = (ang-angp)/Ts;
riobrien 0:1de151a3fae9 255
riobrien 0:1de151a3fae9 256 // Age variables
riobrien 0:1de151a3fae9 257 angp = ang;
riobrien 0:1de151a3fae9 258
riobrien 0:1de151a3fae9 259 // Drive motor using PI ctrl
riobrien 0:1de151a3fae9 260 err = dspeed-speed;
riobrien 0:1de151a3fae9 261 dc = dc_prev + Kpi*(1+zpi*Ts/2)*err - Kpi*(1-zpi*Ts/2)*err_prev;
riobrien 0:1de151a3fae9 262
riobrien 0:1de151a3fae9 263 // Age variables
riobrien 0:1de151a3fae9 264 dc_prev = dc;
riobrien 0:1de151a3fae9 265 err_prev = err;
riobrien 0:1de151a3fae9 266
riobrien 0:1de151a3fae9 267 // Apply saturation
riobrien 0:1de151a3fae9 268 if (dc > 1.0){dc = 1.0;}
riobrien 0:1de151a3fae9 269 else {
riobrien 0:1de151a3fae9 270 if (dc < -1.0){dc = -1.0;}
riobrien 0:1de151a3fae9 271 }
riobrien 0:1de151a3fae9 272
riobrien 0:1de151a3fae9 273 // Stop motor at t = 1 sec
riobrien 0:1de151a3fae9 274 if (cntr >= 1000) {
riobrien 0:1de151a3fae9 275 dc = 0.0;}
riobrien 0:1de151a3fae9 276
riobrien 0:1de151a3fae9 277
riobrien 0:1de151a3fae9 278 // Log data
riobrien 0:1de151a3fae9 279 etime[k] = cntr*Ts;
riobrien 0:1de151a3fae9 280 ang_pos[k] = ang;
riobrien 0:1de151a3fae9 281 est_speed[k] = speed;
riobrien 0:1de151a3fae9 282 dc_in[k] = dc;
riobrien 0:1de151a3fae9 283 k++;
riobrien 0:1de151a3fae9 284 //dc = -1;
riobrien 0:1de151a3fae9 285
riobrien 0:1de151a3fae9 286 mot1_ph1 = 0;
riobrien 0:1de151a3fae9 287 mot_en1 = abs(dc);
riobrien 0:1de151a3fae9 288
riobrien 0:1de151a3fae9 289
riobrien 0:1de151a3fae9 290
riobrien 0:1de151a3fae9 291 t.stop(); // end measuring comp time
riobrien 0:1de151a3fae9 292 dt = Ts-t.read();
riobrien 0:1de151a3fae9 293 //printf("%5.2f\n\r",cntr);
riobrien 0:1de151a3fae9 294 pc.printf("%5.2f %5.2f %5.2f %5.2f \n\r",cntr*Ts,speed,dc,err);
riobrien 0:1de151a3fae9 295 //pc.printf("%5.2f %d \n\r",cntr*Ts,enc1);
riobrien 0:1de151a3fae9 296
riobrien 0:1de151a3fae9 297 t.reset();
riobrien 0:1de151a3fae9 298 cntr=cntr+1;
riobrien 0:1de151a3fae9 299 wait(dt); // wait to ensure sampling period set by Ts
riobrien 0:1de151a3fae9 300 //if (cntr == 200){fclose(fp);}
riobrien 0:1de151a3fae9 301
riobrien 0:1de151a3fae9 302 }//while
riobrien 0:1de151a3fae9 303
riobrien 0:1de151a3fae9 304 // Print out log variables in MATLAB structured variable format.
riobrien 0:1de151a3fae9 305 pc.printf("Printing log variables to file on mBed ... ");
riobrien 0:1de151a3fae9 306
riobrien 0:1de151a3fae9 307 dc_in[0] = Kpi*(1+zpi*Ts/2)*dspeed;
riobrien 0:1de151a3fae9 308
riobrien 0:1de151a3fae9 309 if(1) {
riobrien 0:1de151a3fae9 310 for(k=0; k<1000; k++) {
riobrien 0:1de151a3fae9 311 fprintf(fp,"t(%d,1) = %.5f;\n",k+1,etime[k]);
riobrien 0:1de151a3fae9 312 fprintf(fp,"est_speed(%d,1) = %.5f;\n",k+1,est_speed[k]);
riobrien 0:1de151a3fae9 313 fprintf(fp,"dc_in(%d,1) = %.5f;\n",k+1,dc_in[k]);
riobrien 0:1de151a3fae9 314 }
riobrien 0:1de151a3fae9 315 }
riobrien 0:1de151a3fae9 316 printf("done.\r\n");
riobrien 0:1de151a3fae9 317
riobrien 0:1de151a3fae9 318 // Close file
riobrien 0:1de151a3fae9 319 fclose(fp);
riobrien 0:1de151a3fae9 320
riobrien 0:1de151a3fae9 321 }//main