Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 145 | f9daq | 1 | #include "rs232.h" |
| 2 | #include "MIKRO.h" |
||
| 3 | |||
| 4 | //#define DEBUG |
||
| 5 | |||
| 6 | #define COMWAIT 0.5 |
||
| 7 | #define COMDELAY 0.1 |
||
| 8 | |||
| 9 | static char MIKRO_Send[100], MIKRO_Receive[100]; |
||
| 10 | static char MIKRO_Device, MIKRO_Axes, MIKRO_Response; |
||
| 11 | static int MIKRO_Port; |
||
| 12 | static int nin, nout, rstat; |
||
| 13 | static int MIKRO_type[100]; |
||
| 14 | |||
| 15 | int MIKRO_Cmd (int node, char *cmd) |
||
| 16 | { |
||
| 17 | printf("Command: %1d %s\n",node,cmd); |
||
| 18 | Delay(COMDELAY); |
||
| 19 | FlushInQ (MIKRO_Port); |
||
| 20 | nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd); |
||
| 21 | ComWrt (MIKRO_Port, MIKRO_Send, nout); |
||
| 22 | if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) { |
||
| 23 | nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa); |
||
| 24 | if (nin==2) return (0); |
||
| 25 | } |
||
| 26 | return (-1); |
||
| 27 | } |
||
| 28 | |||
| 29 | int MIKRO_Set (int node, char cmd[], int val) |
||
| 30 | { |
||
| 31 | printf("Command: %1d %s %d\n",node,cmd, val); |
||
| 32 | Delay(COMDELAY); |
||
| 33 | FlushInQ (MIKRO_Port); |
||
| 34 | nout = sprintf (MIKRO_Send, "%1d %s %d\r", node, cmd, val); |
||
| 35 | ComWrt (MIKRO_Port, MIKRO_Send, nout); |
||
| 36 | if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) { |
||
| 37 | nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa); |
||
| 38 | if (nin==2) return (0); |
||
| 39 | } |
||
| 40 | return (-1); |
||
| 41 | } |
||
| 42 | |||
| 43 | int MIKRO_Get (int node, char cmd[], int *val) |
||
| 44 | { |
||
| 45 | short int stmp; |
||
| 46 | |||
| 47 | Delay(COMDELAY); |
||
| 48 | FlushInQ (MIKRO_Port); |
||
| 49 | nout = sprintf (MIKRO_Send, "%1d %s\r", node, cmd); |
||
| 50 | ComWrt (MIKRO_Port, MIKRO_Send, nout); |
||
| 51 | if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) { |
||
| 52 | nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa); |
||
| 53 | if (nin>0){ |
||
| 54 | // MIKRO_Receive[--nin]=0; |
||
| 55 | switch (nin) { |
||
| 56 | case 9: |
||
| 57 | sscanf (MIKRO_Receive, "%*x %hx",&stmp); |
||
| 58 | *val=stmp; |
||
| 59 | return (0); |
||
| 60 | case 13: |
||
| 61 | sscanf (MIKRO_Receive, "%*x %x",val); |
||
| 62 | return (0); |
||
| 63 | default: |
||
| 64 | printf("Node %d Com error => bytes rcved=0x%02x buf=%s\n",node,nin,MIKRO_Receive); |
||
| 65 | break; |
||
| 66 | } |
||
| 67 | } |
||
| 68 | } |
||
| 69 | return (-1); |
||
| 70 | } |
||
| 71 | |||
| 72 | int MIKRO_GetStat (int node) |
||
| 73 | { |
||
| 74 | int tmp; |
||
| 75 | |||
| 76 | Delay(COMDELAY); |
||
| 77 | FlushInQ (MIKRO_Port); |
||
| 78 | nout = sprintf (MIKRO_Send, "%1d st\r", node); |
||
| 79 | ComWrt (MIKRO_Port, MIKRO_Send, nout); |
||
| 80 | if ((nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa))==0) { |
||
| 81 | nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa); |
||
| 82 | // if (nin>0) nin--; |
||
| 83 | MIKRO_Receive[nin]=0; |
||
| 84 | if (nin==9) { |
||
| 85 | tmp=0; |
||
| 86 | sscanf (MIKRO_Receive, "%*x %hx",(short int *)&tmp); |
||
| 87 | return (tmp); |
||
| 88 | } |
||
| 89 | } |
||
| 90 | return (-1); |
||
| 91 | } |
||
| 92 | |||
| 93 | int _VI_FUNC MIKRO_Open (char * dev) |
||
| 94 | { |
||
| 95 | |||
| 96 | |||
| 97 | MIKRO_Port=OpenComConfig (dev, "", 38400, 0, 8, 1, 512, 512); |
||
| 98 | // SetXMode (MIKRO_Port, 0); |
||
| 99 | // SetCTSMode (MIKRO_Port, LWRS_HWHANDSHAKE_OFF); |
||
| 100 | // SetComTime (MIKRO_Port, COMWAIT); |
||
| 101 | |||
| 102 | return 0; |
||
| 103 | } |
||
| 104 | |||
| 105 | int _VI_FUNC MIKRO_Init (int node, int type) |
||
| 106 | { |
||
| 107 | MIKRO_type[node]=type; |
||
| 108 | Delay(0.1); |
||
| 109 | MIKRO_Cmd(node,"ok 1"); |
||
| 110 | MIKRO_Cmd(node,"ab"); |
||
| 111 | switch (type){ |
||
| 112 | case 1: // 3M Linear |
||
| 113 | MIKRO_Cmd(node,"k 1"); |
||
| 114 | MIKRO_Cmd(node,"ad 200"); |
||
| 115 | MIKRO_Cmd(node,"aa 2"); |
||
| 116 | MIKRO_Cmd(node,"fa 1"); |
||
| 117 | MIKRO_Cmd(node,"fd 3000"); // Set Max Dynamic Following Error (1000) |
||
| 118 | MIKRO_Cmd(node,"sr 1000"); |
||
| 119 | MIKRO_Cmd(node,"sp 750"); |
||
| 120 | MIKRO_Cmd(node,"ac 100"); |
||
| 121 | MIKRO_Cmd(node,"dc 200"); |
||
| 122 | MIKRO_Cmd(node,"por 28000"); |
||
| 123 | MIKRO_Cmd(node,"i 600"); |
||
| 124 | MIKRO_Cmd(node,"ano 2350"); |
||
| 125 | MIKRO_Cmd(node,"ls 1"); |
||
| 126 | MIKRO_Cmd(node,"hp 1"); |
||
| 127 | MIKRO_Cmd(node,"hf 1"); |
||
| 128 | break; |
||
| 129 | case 2: // 3M Rotary |
||
| 130 | MIKRO_Cmd(node,"k 1"); |
||
| 131 | MIKRO_Cmd(node,"ad 200"); |
||
| 132 | MIKRO_Cmd(node,"aa 1"); |
||
| 133 | MIKRO_Cmd(node,"fa 1"); |
||
| 134 | MIKRO_Cmd(node,"fd 3000"); // Set Max Dynamic Following Error (1000) |
||
| 135 | MIKRO_Cmd(node,"sr 1000"); |
||
| 136 | MIKRO_Cmd(node,"sp 550"); |
||
| 137 | MIKRO_Cmd(node,"ac 100"); |
||
| 138 | MIKRO_Cmd(node,"dc 200"); |
||
| 139 | MIKRO_Cmd(node,"por 28000"); |
||
| 140 | MIKRO_Cmd(node,"i 600"); |
||
| 141 | MIKRO_Cmd(node,"ano 2350"); |
||
| 142 | MIKRO_Cmd(node,"ls 99"); |
||
| 143 | MIKRO_Cmd(node,"hp 1"); |
||
| 144 | MIKRO_Cmd(node,"hf 1"); |
||
| 145 | break; |
||
| 146 | case 3: // 4M Linear |
||
| 147 | MIKRO_Cmd(node,"k 1"); |
||
| 148 | MIKRO_Cmd(node,"ad 1000"); |
||
| 149 | MIKRO_Cmd(node,"aa 2"); |
||
| 150 | MIKRO_Cmd(node,"fa 1"); |
||
| 151 | MIKRO_Cmd(node,"fd 3000"); // Set Max Dynamic Following Error (1000) |
||
| 152 | MIKRO_Cmd(node,"sr 1000"); |
||
| 153 | MIKRO_Cmd(node,"sp 1000"); |
||
| 154 | MIKRO_Cmd(node,"ac 100"); |
||
| 155 | MIKRO_Cmd(node,"dc 200"); |
||
| 156 | MIKRO_Cmd(node,"por 28000"); |
||
| 157 | MIKRO_Cmd(node,"i 600"); |
||
| 158 | MIKRO_Cmd(node,"ano 2600"); |
||
| 159 | MIKRO_Cmd(node,"ls 1"); |
||
| 160 | MIKRO_Cmd(node,"hp 1"); |
||
| 161 | MIKRO_Cmd(node,"hf 1"); |
||
| 162 | break; |
||
| 163 | case 4: // 4M Rotary |
||
| 164 | MIKRO_Cmd(node,"k 1"); |
||
| 165 | MIKRO_Cmd(node,"ad 100"); |
||
| 166 | MIKRO_Cmd(node,"aa 1"); |
||
| 167 | MIKRO_Cmd(node,"fa 1"); |
||
| 168 | MIKRO_Cmd(node,"fd 3000"); // Set Max Dynamic Following Error (1000) |
||
| 169 | MIKRO_Cmd(node,"sp 800"); |
||
| 170 | MIKRO_Cmd(node,"sr 1000"); |
||
| 171 | MIKRO_Cmd(node,"ac 100"); |
||
| 172 | MIKRO_Cmd(node,"dc 200"); |
||
| 173 | MIKRO_Cmd(node,"por 28000"); |
||
| 174 | MIKRO_Cmd(node,"i 600"); |
||
| 175 | MIKRO_Cmd(node,"ano 2600"); |
||
| 176 | MIKRO_Cmd(node,"ls 99"); |
||
| 177 | break; |
||
| 178 | default: |
||
| 179 | break; |
||
| 180 | } |
||
| 181 | MIKRO_Cmd(node,"rd 0"); |
||
| 182 | MIKRO_Cmd(node,"n 2"); |
||
| 183 | MIKRO_Cmd(node,"en"); |
||
| 184 | if (type != 0){ |
||
| 185 | MIKRO_Cmd(node,"eeboot 1"); |
||
| 186 | MIKRO_Cmd(node,"eepsav 1"); |
||
| 187 | Delay(0.1); |
||
| 188 | } |
||
| 189 | return 0; |
||
| 190 | } |
||
| 191 | |||
| 192 | int _VI_FUNC MIKRO_Reset (int node) |
||
| 193 | { |
||
| 194 | |||
| 195 | MIKRO_Cmd(node,"di"); // disables the node |
||
| 196 | |||
| 197 | Delay(COMDELAY); |
||
| 198 | nout = sprintf (MIKRO_Send, "%1d rn\r", node); // resets the node |
||
| 199 | ComWrt (MIKRO_Port, MIKRO_Send, nout); |
||
| 200 | |||
| 201 | SetComTime (MIKRO_Port, 20); |
||
| 202 | nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa); |
||
| 203 | SetComTime (MIKRO_Port, COMWAIT); |
||
| 204 | nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa); |
||
| 205 | // if (nin!=0) nin--; |
||
| 206 | // MIKRO_Receive[nin]=0; |
||
| 207 | printf("%s\n",MIKRO_Receive); |
||
| 208 | nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa); |
||
| 209 | nin = ComRdTerm (MIKRO_Port, MIKRO_Receive, 30, 0xa); |
||
| 210 | // if (nin!=0) nin--; |
||
| 211 | // MIKRO_Receive[nin]=0; |
||
| 212 | printf("%s\n",MIKRO_Receive); |
||
| 213 | MIKRO_Init(node,0); |
||
| 214 | return 0; |
||
| 215 | } |
||
| 216 | |||
| 217 | int _VI_FUNC MIKRO_ReferenceMove (int node) |
||
| 218 | { |
||
| 219 | int fac=10; |
||
| 220 | int n2,as; |
||
| 221 | |||
| 222 | MIKRO_Cmd(node,"ab"); |
||
| 223 | MIKRO_Cmd(node,"en"); |
||
| 224 | MIKRO_Set(node,"ll",-1000000); |
||
| 225 | MIKRO_Set(node,"ll",1000000); |
||
| 226 | |||
| 227 | if (!(MIKRO_GetStat(node)&0x8000)){ |
||
| 228 | MIKRO_Set(node,"v", -100*fac); |
||
| 229 | do { |
||
| 230 | MIKRO_GetPosition(node,&n2); |
||
| 231 | MIKRO_Get(node,"as",&as); |
||
| 232 | printf("Approaching N-limit node=%d pos=%d speed=%d\n",node,n2,as); |
||
| 233 | } while (MIKRO_GetStat(node)&0x1 ); |
||
| 234 | if (!(MIKRO_GetStat(node)&0x8000)){ |
||
| 235 | printf("N-limit not reached! Trying with half speed.\n"); |
||
| 236 | MIKRO_Set(node,"v", -50*fac); |
||
| 237 | do { |
||
| 238 | MIKRO_GetPosition(node,&n2); |
||
| 239 | MIKRO_Get(node,"as",&as); |
||
| 240 | printf("Approaching N-limit node=%d pos=%d speed=%d\n",node,n2,as); |
||
| 241 | } while (MIKRO_GetStat(node)&0x1 ); |
||
| 242 | if (!(MIKRO_GetStat(node)&0x8000)){ |
||
| 243 | printf("N-limit not reached! Aborting ...\n"); |
||
| 244 | MIKRO_Cmd(node,"ab"); |
||
| 245 | return -1; |
||
| 246 | } |
||
| 247 | } |
||
| 248 | } |
||
| 249 | MIKRO_MoveFor(node,1000); |
||
| 250 | MIKRO_Set(node,"v", -10*fac); |
||
| 251 | do { |
||
| 252 | MIKRO_GetPosition(node,&n2); |
||
| 253 | MIKRO_Get(node,"as",&as); |
||
| 254 | printf("Fine tuning 0: node=%d pos=%d speed=%d\n",node,n2, as); |
||
| 255 | } while (MIKRO_GetStat(node)&0x1); |
||
| 256 | if (!(MIKRO_GetStat(node)&0x8000)){ |
||
| 257 | printf("N-limit not reached! Aborting ...\n"); |
||
| 258 | MIKRO_Cmd(node,"ab"); |
||
| 259 | return -1; |
||
| 260 | } |
||
| 261 | |||
| 262 | MIKRO_MoveFor(node,1000); |
||
| 263 | MIKRO_Set(node,"ho",0); |
||
| 264 | MIKRO_Set(node,"ll",-100); |
||
| 265 | MIKRO_Set(node,"ll",500100); |
||
| 266 | return 0; |
||
| 267 | } |
||
| 268 | |||
| 269 | int _VI_FUNC MIKRO_MoveFor (int node, int dist) |
||
| 270 | { |
||
| 271 | MIKRO_Set(node,"lr", dist); |
||
| 272 | MIKRO_Cmd(node,"mv"); |
||
| 273 | while (MIKRO_GetStat(node)&1) Delay(0.1); |
||
| 274 | return 0; |
||
| 275 | } |
||
| 276 | |||
| 277 | int _VI_FUNC MIKRO_MoveTo (int node, int dest) |
||
| 278 | { |
||
| 279 | // printf("-> MIKRO_MoveTo \n"); |
||
| 280 | MIKRO_Set(node,"la", dest); |
||
| 281 | MIKRO_Cmd(node,"mv"); |
||
| 282 | while (MIKRO_GetStat(node)&1) Delay(0.1); |
||
| 283 | return 0; |
||
| 284 | } |
||
| 285 | |||
| 286 | int _VI_FUNC MIKRO_GetPosition (int node, int pos[]) |
||
| 287 | { |
||
| 288 | MIKRO_Get(node,"pos",pos); |
||
| 289 | return 0; |
||
| 290 | } |
||
| 291 | |||
| 292 | void _VI_FUNC MIKRO_Close (void) |
||
| 293 | { |
||
| 294 | CloseCom (MIKRO_Port); |
||
| 295 | } |
||
| 296 |