Changeset 356

Show
Ignore:
Timestamp:
09/30/08 12:45:29 (5 years ago)
Author:
atorf
Message:

New NXC source for mini hybrid program

Location:
branches/atorf/NXC
Files:
2 modified

Legend:

Unmodified
Added
Removed

  • branches/atorf/NXC/MotorControl.nxc

    r355 r356  
    11#define INBOX 1 
    22 
     3 
     4struct RotateMotorInfo { 
     5    int power; 
     6    long angle; 
     7    int turnratio; 
     8    bool syncmode; 
     9    bool endbrake; 
     10}; 
     11 
     12RotateMotorInfo motorA; 
     13RotateMotorInfo motorB; 
     14RotateMotorInfo motorC; 
     15RotateMotorInfo motorsSync; 
     16 
     17mutex movingA; 
     18mutex movingB; 
     19mutex movingC; 
     20byte SyncPorts; 
     21 
     22task MoveA() { 
     23 
     24    Acquire(movingA); 
     25        RotateMotorEx(OUT_A, motorA.power, motorA.angle, motorA.turnratio, motorA.syncmode, motorA.endbrake); 
     26    Release(movingA); 
     27 
     28}//MoveA 
     29 
     30task MoveB() { 
     31 
     32    Acquire(movingB); 
     33        RotateMotorEx(OUT_B, motorB.power, motorB.angle, motorB.turnratio, motorB.syncmode, motorB.endbrake); 
     34    Release(movingB); 
     35 
     36}//MoveB 
     37 
     38task MoveC() { 
     39 
     40    Acquire(movingC); 
     41        RotateMotorEx(OUT_C, motorC.power, motorC.angle, motorC.turnratio, motorC.syncmode, motorC.endbrake); 
     42    Release(movingC); 
     43 
     44}//MoveC 
     45 
     46 
     47task MoveSync() { 
     48 
     49    if (SyncPorts == 3) { 
     50        Acquire(movingA); 
     51        Acquire(movingB); 
     52            RotateMotorEx(OUT_AB, motorsSync.power, motorsSync.angle, motorsSync.turnratio, motorsSync.syncmode, motorsSync.endbrake); 
     53        Release(movingA); 
     54        Release(movingB); 
     55    } else if (SyncPorts == 4) { 
     56        Acquire(movingA); 
     57        Acquire(movingC); 
     58            RotateMotorEx(OUT_AC, motorsSync.power, motorsSync.angle, motorsSync.turnratio, motorsSync.syncmode, motorsSync.endbrake); 
     59        Release(movingA); 
     60        Release(movingC); 
     61    } else if (SyncPorts == 5) { 
     62        Acquire(movingB); 
     63        Acquire(movingC); 
     64            RotateMotorEx(OUT_BC, motorsSync.power, motorsSync.angle, motorsSync.turnratio, motorsSync.syncmode, motorsSync.endbrake); 
     65        Release(movingB); 
     66        Release(movingC); 
     67    }//end if 
     68 
     69 
     70 
     71}//MoveC 
     72 
     73 
     74void ResetAllCounters(byte port, int pwr, long tacholimit, int turnratio, bool speedreg, bool sync) { 
     75 
     76    byte flags = UF_UPDATE_RESET_COUNT; 
     77 
     78    byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON; 
     79    byte reg  = OUT_REGMODE_IDLE; 
     80    byte state = OUT_RUNSTATE_RUNNING; 
     81 
     82    if (speedreg) { 
     83        mode = mode + OUT_MODE_REGULATED; 
     84        reg  = OUT_REGMODE_SPEED; 
     85    }//end if 
     86 
     87    if (sync) { 
     88        mode = mode + OUT_MODE_REGULATED; 
     89        reg  = OUT_REGMODE_SYNC; 
     90    }//end if 
     91 
     92 
     93    SetOutput(port, Power, pwr, TachoLimit, tacholimit, TurnRatio, turnratio, OutputMode, mode, RegMode, reg,  RunState, state, UpdateFlags, flags); 
     94 
     95 
     96} // end MotorCmd 
     97 
     98 
     99 
     100 
     101void MotorCmd(byte port, int pwr, long tacholimit, int turnratio, bool speedreg, bool sync) { 
     102 
     103    byte flags = UF_UPDATE_MODE + UF_UPDATE_RESET_COUNT + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT; 
     104 
     105    byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON; 
     106    byte reg  = OUT_REGMODE_IDLE; 
     107    byte state = OUT_RUNSTATE_RUNNING; 
     108 
     109    if (speedreg) { 
     110        mode = mode + OUT_MODE_REGULATED; 
     111        reg  = OUT_REGMODE_SPEED; 
     112    }//end if 
     113 
     114    if (sync) { 
     115        mode = mode + OUT_MODE_REGULATED; 
     116        reg  = OUT_REGMODE_SYNC; 
     117    }//end if 
     118 
     119 
     120    SetOutput(port, Power, pwr, TachoLimit, tacholimit, TurnRatio, turnratio, OutputMode, mode, RegMode, reg,  RunState, state, UpdateFlags, flags); 
     121 
     122 
     123} // end MotorCmd 
     124 
     125 
    3126task main(){ 
    4     string in; 
     127 
     128    string in = ""; 
    5129    int i = 0; 
    6     int  port  = -1; 
     130    byte  port; 
    7131    int  power = 0; 
    8132    long angle = 0; 
    … …  
    11135    bool syncmode = false; 
    12136    bool endbrake = false; 
    13     string synctext; 
    14     string braketext; 
    15     string msg; 
    16     string tmp; 
    17     string tmp2; 
     137    string synctext = ""; 
     138    string braketext = ""; 
     139    string msg = ""; 
     140    string tmp = ""; 
     141    string tmp2 = ""; 
    18142     
    19143    while(true){ 
    … …  
    25149            // read values 
    26150             
    27  
    28151            tmp      = SubStr(in,  0, 1);  // pos 0 
    29152            port     = StrToNum(tmp); 
    … …  
    41164            iMode    = StrToNum(tmp); 
    42165 
    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); 
     166 
     167             
     168 
     169 
     170 
     171            if (power == 201) { 
     172                if (port <= 2) { 
     173                    ResetAllTachoCounts(port); 
     174                } else if (port == 6) { 
     175                    ResetAllTachoCounts(OUT_A); 
     176                    ResetAllTachoCounts(OUT_B); 
     177                    ResetAllTachoCounts(OUT_C); 
     178                }//end if 
     179                PlayTone(1200, 50); 
     180            } else { 
     181 
     182               // process... 
     183 
     184                if(power > 100) { 
     185                    power = -(power - 100); 
     186                }//end if 
     187 
     188                if(turnratio > 100) { 
     189                    turnratio = -(turnratio - 100); 
     190                }//end if 
     191 
     192                synctext = "syncOFF"; 
     193                braketext = "endCOAST"; 
     194                if (iMode == 0) { 
     195                    syncmode = false; 
     196                    endbrake = false; 
     197                } else if (iMode == 1) { 
     198                    syncmode = false; 
     199                    endbrake = true; 
     200                    braketext = "endBRAKE"; 
     201                } else if (iMode == 2) { 
     202                    synctext = "syncON"; 
     203                    syncmode = true; 
     204                    endbrake = false; 
     205                } else if (iMode == 3) { 
     206                    synctext = "syncON"; 
     207                    braketext = "endBRAKE"; 
     208                    syncmode = true; 
     209                    endbrake = true; 
     210                }// end if 
     211 
     212 
     213 
     214                // debug out 
     215 
     216 
     217                tmp = NumToStr(i); 
     218                msg = StrCat("MotorCmdNr=", tmp); 
     219                TextOut(0,LCD_LINE1, msg, true); 
     220                tmp = NumToStr(port); 
     221                tmp2 = NumToStr(power); 
     222                msg = StrCat("Port=", tmp, " Pwr=", tmp2); 
     223                TextOut(0,LCD_LINE3, msg); 
     224                tmp = NumToStr(angle); 
     225                tmp2 = NumToStr(turnratio); 
     226                msg = StrCat("Angl=", tmp, " Turn=", tmp2); 
     227                TextOut(0,LCD_LINE5, msg); 
     228                msg = StrCat(synctext, " ", braketext); 
     229                TextOut(0,LCD_LINE7, msg); 
     230 
     231 
     232 
     233                // finally, the command 
     234 
     235                if (port == 0) { 
     236 
     237                    motorA.power = power; 
     238                    motorA.angle = angle; 
     239                    motorA.turnratio = turnratio; 
     240                    motorA.syncmode = syncmode; 
     241                    motorA.endbrake = endbrake; 
     242 
     243                    start MoveA; 
     244 
     245                } else if (port == 1) { 
     246 
     247                    motorB.power = power; 
     248                    motorB.angle = angle; 
     249                    motorB.turnratio = turnratio; 
     250                    motorB.syncmode = syncmode; 
     251                    motorB.endbrake = endbrake; 
     252 
     253                    start MoveB; 
     254 
     255 
     256                } else if (port == 2) { 
     257 
     258                    motorC.power = power; 
     259                    motorC.angle = angle; 
     260                    motorC.turnratio = turnratio; 
     261                    motorC.syncmode = syncmode; 
     262                    motorC.endbrake = endbrake; 
     263 
     264                    start MoveC; 
     265                } else { 
     266 
     267                    SyncPorts = port; 
     268 
     269                    motorsSync.power = power; 
     270                    motorsSync.angle = angle; 
     271                    motorsSync.turnratio = turnratio; 
     272                    motorsSync.syncmode = syncmode; 
     273                    motorsSync.endbrake = endbrake; 
     274 
     275                    start MoveSync; 
     276 
     277                }//end if 
     278 
     279                //RotateMotorEx(port, power, angle, turnratio, syncmode, endbrake); 
     280 
     281 
     282                PlayTone(440, 50); 
     283 
     284 
     285            } // end if 
    92286             
    93287            // reset!!!! 
    … …  
    105299    }//end while 
    106300} 
     301 
     302 
     303 

  • branches/atorf/NXC/test.nxc

    r290 r356  
    1 task main() 
    2 { 
    3      OnFwd(OUT_A, 75); 
    4      OnFwd(OUT_C, 75); 
    5      Wait(4000); 
    6      OnRev(OUT_AC, 75); 
    7      Wait(4000); 
    8      Off(OUT_AC); 
     1 
     2void MotorCmd(byte port, int pwr, long tacholimit, int turnratio, bool speedreg, bool sync) { 
     3 
     4    byte flags = UF_UPDATE_MODE + UF_UPDATE_RESET_COUNT + UF_UPDATE_SPEED + UF_UPDATE_TACHO_LIMIT; 
     5 
     6    byte mode = OUT_MODE_BRAKE + OUT_MODE_MOTORON; 
     7    byte reg  = OUT_REGMODE_IDLE; 
     8    byte state = OUT_RUNSTATE_RUNNING; 
     9 
     10    if (speedreg) { 
     11        mode = mode + OUT_MODE_REGULATED; 
     12        reg  = OUT_REGMODE_SPEED; 
     13    }//end if 
     14 
     15    if (sync) { 
     16        mode = mode + OUT_MODE_REGULATED; 
     17        reg  = OUT_REGMODE_SYNC; 
     18    }//end if 
     19 
     20 
     21    SetOutput(port, Power, pwr, TachoLimit, tacholimit, TurnRatio, turnratio, OutputMode, mode, RegMode, reg,  RunState, state, UpdateFlags, flags); 
     22 
     23 
     24 
     25} // end MotorCmd 
     26 
     27 
     28task main(){ 
     29 
     30 
     31    int power = 50; 
     32 
     33    //MotorCmd(OUT_A, 50, 90, 0, true, false); 
     34    RotateMotor(OUT_B, power, 500); 
     35    PlayTone(440, 100); 
     36    Wait(2000); 
     37    PlayTone(300, 100); 
    938} 
    1039 
     40 
     41