USNA ES305 / Mbed 2 deprecated Lab6

Lab 6 code

Dependencies:   mbed

main.cpp@0:5d2df7452db5, 2014-10-10 (annotated)

Committer:
riobrien
Date:
Fri Oct 10 19:20:07 2014 +0000
Revision:
0:5d2df7452db5
Child:
1:49d9a5382ca9
None

Who changed what in which revision?

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