root/branches/atorf/NXC/MotorControl.nxc @ 355

Revision 355, 3.0 KB (checked in by atorf, 5 years ago)

More research (and a little GUI) for NXC hybrid motor control
Line 
1#define INBOX 1
2
3task main(){
4    string in;
5    int i = 0;
6    int  port  = -1;
7    int  power = 0;
8    long angle = 0;
9    int  turnratio  = 0;
10    int iMode = 0;
11    bool syncmode = false;
12    bool endbrake = false;
13    string synctext;
14    string braketext;
15    string msg;
16    string tmp;
17    string tmp2;
18   
19    while(true){
20
21        ReceiveRemoteString(INBOX, true, in);
22
23        if(StrLen(in) > 0) {
24       
25            // read values
26           
27
28            tmp      = SubStr(in,  0, 1);  // pos 0
29            port     = StrToNum(tmp);
30           
31            tmp      = SubStr(in,  1, 3);  // pos 1 2 3
32            power    = StrToNum(tmp);
33           
34            tmp      = SubStr(in,  4, 5);  // pos 4 5 6 7 8
35            angle    = StrToNum(tmp);
36           
37            tmp      = SubStr(in,  9, 3);  // pos 9 10 11
38            turnratio= StrToNum(tmp);
39           
40            tmp      = SubStr(in,  12, 1); // pos 12
41            iMode    = StrToNum(tmp);
42
43            // process...
44           
45            if(power > 100) {
46                port = -(power - 100);
47            }//end if
48            if(turnratio > 100) {
49                turnratio = -(turnratio - 100);
50            }//end if
51           
52            synctext = "SYNC off";
53            braketext = "COAST at end";
54            if (iMode == 0) {
55                syncmode = false;
56                endbrake = false;
57            } else if (iMode == 1) {
58                syncmode = false;
59                endbrake = true;
60                braketext = "BRAKE at end";
61            } else if (iMode == 2) {
62                synctext = "SYNC on";
63                syncmode = true;
64                endbrake = false;
65            } else if (iMode == 3) {
66                synctext = "SYNC on";
67                braketext = "BRAKE at end";
68                syncmode = true;
69                endbrake = true;
70            }// end if
71
72            // debug out
73           
74           
75            tmp = NumToStr(i);
76            msg = StrCat("Received motor command nr. ", tmp);
77            TextOut(10,LCD_LINE1, msg, true);
78            tmp = NumToStr(port);
79            tmp2 = NumToStr(power);
80            msg = StrCat("Port = 0x", tmp, "  Power = ", tmp2, "%");
81            TextOut(10,LCD_LINE3, msg);
82            tmp = NumToStr(angle);
83            tmp2 = NumToStr(power);
84            msg = StrCat("Angle = ", tmp, "  TurnRatio = ", tmp2);
85            TextOut(10,LCD_LINE5, msg);
86            msg = StrCat(synctext, "   ", braketext);
87            TextOut(10,LCD_LINE7, msg);
88           
89            // finally, the command
90           
91            RotateMotorEx(port, power, angle, turnratio, syncmode, endbrake);
92           
93            // reset!!!!
94           
95            in = "";
96           
97            // count ...
98            i++;
99            if (i > 32000) { // anti-overflow
100                i = 0;
101            }//end if
102           
103        }//end if
104       
105    }//end while
106}
Note: See TracBrowser for help on using the browser.