2 Achsen Motorsteuerung mit Fehlerauswertung
Dependencies: EPOS2 mbed-rtos mbed
main.cpp@1:c53aafa72b36, 2016-07-28 (annotated)
- Committer:
- ODEM
- Date:
- Thu Jul 28 11:50:16 2016 +0000
- Revision:
- 1:c53aafa72b36
- Parent:
- 0:aaf583a75b97
Fehlerauswertung ; 2 Achsen Motorsteuerung
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ODEM | 0:aaf583a75b97 | 1 | #include "mbed.h" |
ODEM | 0:aaf583a75b97 | 2 | #include "EPOS2.h" |
ODEM | 0:aaf583a75b97 | 3 | //#include "RobotControl.h" |
ODEM | 0:aaf583a75b97 | 4 | |
ODEM | 0:aaf583a75b97 | 5 | //***********************************************************************************************************************************// |
ODEM | 0:aaf583a75b97 | 6 | //*** Global_Var *******************************************************************************************************************// |
ODEM | 0:aaf583a75b97 | 7 | //***********************************************************************************************************************************// |
ODEM | 0:aaf583a75b97 | 8 | |
ODEM | 0:aaf583a75b97 | 9 | Serial pc(USBTX, USBRX); // (tx, rx) |
ODEM | 0:aaf583a75b97 | 10 | |
ODEM | 0:aaf583a75b97 | 11 | |
ODEM | 0:aaf583a75b97 | 12 | CAN can(p9, p10); // Can Pin def |
ODEM | 0:aaf583a75b97 | 13 | |
ODEM | 0:aaf583a75b97 | 14 | |
ODEM | 0:aaf583a75b97 | 15 | int velocity = 3000; |
ODEM | 0:aaf583a75b97 | 16 | int acceleration = 50000; |
ODEM | 0:aaf583a75b97 | 17 | int deceleration = 50000; |
ODEM | 0:aaf583a75b97 | 18 | |
ODEM | 0:aaf583a75b97 | 19 | int anzahl_zyklen = 1000000; |
ODEM | 0:aaf583a75b97 | 20 | int absolvierte_zyklen = 0; |
ODEM | 0:aaf583a75b97 | 21 | int ActualPos = 0; |
ODEM | 0:aaf583a75b97 | 22 | int DemandPos = 0; |
ODEM | 0:aaf583a75b97 | 23 | int ActualCurrent = 0; |
ODEM | 0:aaf583a75b97 | 24 | int DemandCurrent = 0; |
ODEM | 0:aaf583a75b97 | 25 | int counter=0; |
ODEM | 1:c53aafa72b36 | 26 | int counter2=0; |
ODEM | 0:aaf583a75b97 | 27 | bool start = true; |
ODEM | 1:c53aafa72b36 | 28 | int current_State = 2; |
ODEM | 1:c53aafa72b36 | 29 | int counter_s = false; |
ODEM | 1:c53aafa72b36 | 30 | int counter_s2 = false; |
ODEM | 0:aaf583a75b97 | 31 | |
ODEM | 0:aaf583a75b97 | 32 | |
ODEM | 0:aaf583a75b97 | 33 | |
ODEM | 0:aaf583a75b97 | 34 | void Motor1 (EPOS2 MyEpos){ |
ODEM | 0:aaf583a75b97 | 35 | pc.printf("Motor1\n"); |
ODEM | 0:aaf583a75b97 | 36 | |
ODEM | 0:aaf583a75b97 | 37 | |
ODEM | 0:aaf583a75b97 | 38 | MyEpos.SetDigOut(3, 0); |
ODEM | 0:aaf583a75b97 | 39 | wait(2); |
ODEM | 0:aaf583a75b97 | 40 | |
ODEM | 0:aaf583a75b97 | 41 | |
ODEM | 0:aaf583a75b97 | 42 | |
ODEM | 0:aaf583a75b97 | 43 | |
ODEM | 0:aaf583a75b97 | 44 | // while(1){ |
ODEM | 0:aaf583a75b97 | 45 | |
ODEM | 0:aaf583a75b97 | 46 | |
ODEM | 1:c53aafa72b36 | 47 | // pc.printf("start\n"); |
ODEM | 0:aaf583a75b97 | 48 | MyEpos.SetDigOut(3, 1); //(1=Enable, 0=Disable) |
ODEM | 0:aaf583a75b97 | 49 | MyEpos.MoveAbsolute(500,20,500,deceleration); |
ODEM | 0:aaf583a75b97 | 50 | MyEpos.SetDigOut(3, 0); //(TargetPosition, PosSpeed, acceleration, deceleration) |
ODEM | 0:aaf583a75b97 | 51 | wait(1); |
ODEM | 1:c53aafa72b36 | 52 | // pc.printf("%d\n", MyEpos.GetError()); |
ODEM | 0:aaf583a75b97 | 53 | |
ODEM | 0:aaf583a75b97 | 54 | while (MyEpos.TargetReached() == 0) { |
ODEM | 0:aaf583a75b97 | 55 | ActualPos = MyEpos.GetActualPos(); |
ODEM | 0:aaf583a75b97 | 56 | DemandPos = MyEpos.GetDemandPos(); |
ODEM | 0:aaf583a75b97 | 57 | ActualCurrent = MyEpos.GetActualCurrentAveraged(); |
ODEM | 0:aaf583a75b97 | 58 | DemandCurrent = MyEpos.GetDemandCurrent(); |
ODEM | 0:aaf583a75b97 | 59 | //wait_ms(100); |
ODEM | 0:aaf583a75b97 | 60 | MyEpos.SetDigOut(3, 0); |
ODEM | 0:aaf583a75b97 | 61 | } // pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent); |
ODEM | 0:aaf583a75b97 | 62 | |
ODEM | 0:aaf583a75b97 | 63 | |
ODEM | 0:aaf583a75b97 | 64 | wait(0.5); |
ODEM | 0:aaf583a75b97 | 65 | |
ODEM | 0:aaf583a75b97 | 66 | MyEpos.SetDigOut(3, 1); |
ODEM | 0:aaf583a75b97 | 67 | //wait_ms(100); |
ODEM | 0:aaf583a75b97 | 68 | MyEpos.MoveAbsolute(0,3000,3000,3000); |
ODEM | 0:aaf583a75b97 | 69 | MyEpos.SetDigOut(3, 0); |
ODEM | 0:aaf583a75b97 | 70 | wait(1); |
ODEM | 0:aaf583a75b97 | 71 | |
ODEM | 0:aaf583a75b97 | 72 | |
ODEM | 0:aaf583a75b97 | 73 | while (MyEpos.TargetReached() == 0) { |
ODEM | 0:aaf583a75b97 | 74 | ActualPos = MyEpos.GetActualPos(); |
ODEM | 0:aaf583a75b97 | 75 | DemandPos = MyEpos.GetDemandPos(); |
ODEM | 0:aaf583a75b97 | 76 | ActualCurrent = MyEpos.GetActualCurrent(); |
ODEM | 0:aaf583a75b97 | 77 | DemandCurrent = MyEpos.GetDemandCurrent(); |
ODEM | 0:aaf583a75b97 | 78 | //wait_ms(100); |
ODEM | 0:aaf583a75b97 | 79 | MyEpos.SetDigOut(3, 0); |
ODEM | 0:aaf583a75b97 | 80 | //pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent); |
ODEM | 0:aaf583a75b97 | 81 | |
ODEM | 0:aaf583a75b97 | 82 | } |
ODEM | 1:c53aafa72b36 | 83 | counter=0; |
ODEM | 0:aaf583a75b97 | 84 | //} |
ODEM | 0:aaf583a75b97 | 85 | } |
ODEM | 0:aaf583a75b97 | 86 | |
ODEM | 0:aaf583a75b97 | 87 | |
ODEM | 0:aaf583a75b97 | 88 | void Motor2(EPOS2 MyEpos2){ |
ODEM | 0:aaf583a75b97 | 89 | pc.printf("Motor2\n"); |
ODEM | 0:aaf583a75b97 | 90 | |
ODEM | 0:aaf583a75b97 | 91 | |
ODEM | 0:aaf583a75b97 | 92 | |
ODEM | 0:aaf583a75b97 | 93 | wait_ms(100) ; |
ODEM | 0:aaf583a75b97 | 94 | MyEpos2. MoveVelocity(1000, 80, 1000); |
ODEM | 0:aaf583a75b97 | 95 | wait(0.1); |
ODEM | 0:aaf583a75b97 | 96 | |
ODEM | 0:aaf583a75b97 | 97 | |
ODEM | 1:c53aafa72b36 | 98 | |
ODEM | 0:aaf583a75b97 | 99 | } |
ODEM | 0:aaf583a75b97 | 100 | int main() ///////// Main |
ODEM | 0:aaf583a75b97 | 101 | { |
ODEM | 0:aaf583a75b97 | 102 | pc.baud(9600); |
ODEM | 1:c53aafa72b36 | 103 | |
ODEM | 0:aaf583a75b97 | 104 | wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist |
ODEM | 0:aaf583a75b97 | 105 | |
ODEM | 0:aaf583a75b97 | 106 | pc.printf("Initialisation CAN\n"); |
ODEM | 0:aaf583a75b97 | 107 | |
ODEM | 0:aaf583a75b97 | 108 | can.frequency(1000000); //Define Can baud in bit/s |
ODEM | 0:aaf583a75b97 | 109 | |
ODEM | 0:aaf583a75b97 | 110 | CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec) |
ODEM | 0:aaf583a75b97 | 111 | canOpen.start(); //Start defined CanOpen Network |
ODEM | 0:aaf583a75b97 | 112 | |
ODEM | 0:aaf583a75b97 | 113 | EPOS2 MyEpos(&canOpen, 1); |
ODEM | 1:c53aafa72b36 | 114 | EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID) |
ODEM | 1:c53aafa72b36 | 115 | |
ODEM | 0:aaf583a75b97 | 116 | |
ODEM | 0:aaf583a75b97 | 117 | |
ODEM | 0:aaf583a75b97 | 118 | pc.printf("Initialisation abgeschlossen\n"); |
ODEM | 0:aaf583a75b97 | 119 | |
ODEM | 0:aaf583a75b97 | 120 | while(1){ |
ODEM | 0:aaf583a75b97 | 121 | |
ODEM | 1:c53aafa72b36 | 122 | |
ODEM | 1:c53aafa72b36 | 123 | |
ODEM | 1:c53aafa72b36 | 124 | //if(counter>2){ |
ODEM | 1:c53aafa72b36 | 125 | // MyEpos.Power(0); |
ODEM | 1:c53aafa72b36 | 126 | // MyEpos2.Power(0); |
ODEM | 1:c53aafa72b36 | 127 | // } |
ODEM | 1:c53aafa72b36 | 128 | |
ODEM | 1:c53aafa72b36 | 129 | |
ODEM | 1:c53aafa72b36 | 130 | |
ODEM | 1:c53aafa72b36 | 131 | |
ODEM | 1:c53aafa72b36 | 132 | switch (current_State){ |
ODEM | 1:c53aafa72b36 | 133 | |
ODEM | 1:c53aafa72b36 | 134 | case 1://STATE_NORMAL: |
ODEM | 1:c53aafa72b36 | 135 | |
ODEM | 1:c53aafa72b36 | 136 | if (MyEpos.GetError()==true && counter<3) { |
ODEM | 1:c53aafa72b36 | 137 | pc.printf("FEHLER-Node1!!!\n"); |
ODEM | 0:aaf583a75b97 | 138 | |
ODEM | 1:c53aafa72b36 | 139 | current_State = 3;//ERROR; |
ODEM | 1:c53aafa72b36 | 140 | |
ODEM | 1:c53aafa72b36 | 141 | } |
ODEM | 1:c53aafa72b36 | 142 | if (MyEpos2.GetError()==true&& counter2<3) { |
ODEM | 1:c53aafa72b36 | 143 | pc.printf("FEHLER-Node2!!!\n"); |
ODEM | 0:aaf583a75b97 | 144 | |
ODEM | 1:c53aafa72b36 | 145 | current_State = 5;//ERROR; |
ODEM | 1:c53aafa72b36 | 146 | |
ODEM | 1:c53aafa72b36 | 147 | } |
ODEM | 1:c53aafa72b36 | 148 | |
ODEM | 1:c53aafa72b36 | 149 | |
ODEM | 1:c53aafa72b36 | 150 | if (counter2>2){ |
ODEM | 1:c53aafa72b36 | 151 | MyEpos2.Power(0); |
ODEM | 1:c53aafa72b36 | 152 | MyEpos.Power(0); |
ODEM | 1:c53aafa72b36 | 153 | } |
ODEM | 1:c53aafa72b36 | 154 | |
ODEM | 1:c53aafa72b36 | 155 | if (counter>2){ |
ODEM | 1:c53aafa72b36 | 156 | MyEpos2.Power(0); |
ODEM | 1:c53aafa72b36 | 157 | MyEpos.Power(0); |
ODEM | 1:c53aafa72b36 | 158 | } |
ODEM | 1:c53aafa72b36 | 159 | Motor2(MyEpos2); |
ODEM | 1:c53aafa72b36 | 160 | Motor1(MyEpos); |
ODEM | 1:c53aafa72b36 | 161 | break; |
ODEM | 1:c53aafa72b36 | 162 | case 2:// INITIALISIERUNG: |
ODEM | 1:c53aafa72b36 | 163 | |
ODEM | 1:c53aafa72b36 | 164 | |
ODEM | 1:c53aafa72b36 | 165 | //************** Initialisierung **************// |
ODEM | 1:c53aafa72b36 | 166 | counter_s=false; |
ODEM | 1:c53aafa72b36 | 167 | counter_s2=false; |
ODEM | 1:c53aafa72b36 | 168 | |
ODEM | 1:c53aafa72b36 | 169 | |
ODEM | 1:c53aafa72b36 | 170 | |
ODEM | 1:c53aafa72b36 | 171 | pc.printf("Fehlerfall %d\n", counter) ; |
ODEM | 1:c53aafa72b36 | 172 | pc.printf("Fehlerfall2 %d\n", counter2) ; |
ODEM | 1:c53aafa72b36 | 173 | MyEpos.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding) |
ODEM | 1:c53aafa72b36 | 174 | MyEpos.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset) |
ODEM | 1:c53aafa72b36 | 175 | MyEpos.Reset(); //Reset the EPOS2 |
ODEM | 1:c53aafa72b36 | 176 | wait(2); |
ODEM | 1:c53aafa72b36 | 177 | MyEpos2.SetPar(10,936, 8, 1000, 29.8); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding) |
ODEM | 1:c53aafa72b36 | 178 | MyEpos2.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset) |
ODEM | 1:c53aafa72b36 | 179 | MyEpos2.Reset(); //Reset the EPOS2 |
ODEM | 1:c53aafa72b36 | 180 | wait(2); |
ODEM | 1:c53aafa72b36 | 181 | pc.printf("Homing\n"); |
ODEM | 1:c53aafa72b36 | 182 | |
ODEM | 1:c53aafa72b36 | 183 | MyEpos.Power(1); |
ODEM | 1:c53aafa72b36 | 184 | pc.printf("power\n"); |
ODEM | 1:c53aafa72b36 | 185 | MyEpos2.Power(1); //Enable the Epos for Homing |
ODEM | 1:c53aafa72b36 | 186 | pc.printf("Power2\n"); |
ODEM | 1:c53aafa72b36 | 187 | wait(2); |
ODEM | 1:c53aafa72b36 | 188 | MyEpos.Homing(); |
ODEM | 1:c53aafa72b36 | 189 | |
ODEM | 1:c53aafa72b36 | 190 | |
ODEM | 1:c53aafa72b36 | 191 | } //do Homing and wait untill homing is done // MyEpos.Power(1); |
ODEM | 1:c53aafa72b36 | 192 | wait_ms(100); |
ODEM | 1:c53aafa72b36 | 193 | current_State = 1;//STATE_NORMAL; |
ODEM | 1:c53aafa72b36 | 194 | break; |
ODEM | 1:c53aafa72b36 | 195 | case 3://ERROR1: |
ODEM | 1:c53aafa72b36 | 196 | |
ODEM | 1:c53aafa72b36 | 197 | |
ODEM | 1:c53aafa72b36 | 198 | |
ODEM | 1:c53aafa72b36 | 199 | if (counter_s==false){ |
ODEM | 1:c53aafa72b36 | 200 | counter++; |
ODEM | 1:c53aafa72b36 | 201 | counter_s=true; |
ODEM | 1:c53aafa72b36 | 202 | |
ODEM | 1:c53aafa72b36 | 203 | current_State = 2;// INITIALISIERUNG; |
ODEM | 1:c53aafa72b36 | 204 | } |
ODEM | 1:c53aafa72b36 | 205 | |
ODEM | 1:c53aafa72b36 | 206 | break; |
ODEM | 1:c53aafa72b36 | 207 | |
ODEM | 1:c53aafa72b36 | 208 | case 4: //STATE_OFF: |
ODEM | 1:c53aafa72b36 | 209 | |
ODEM | 1:c53aafa72b36 | 210 | MyEpos.Power(0); |
ODEM | 1:c53aafa72b36 | 211 | MyEpos2.Power(0); |
ODEM | 1:c53aafa72b36 | 212 | |
ODEM | 1:c53aafa72b36 | 213 | break; |
ODEM | 1:c53aafa72b36 | 214 | |
ODEM | 1:c53aafa72b36 | 215 | case 5://ERROR2 |
ODEM | 1:c53aafa72b36 | 216 | if (counter_s2==false){ |
ODEM | 1:c53aafa72b36 | 217 | counter2++; |
ODEM | 1:c53aafa72b36 | 218 | counter_s2=true; |
ODEM | 1:c53aafa72b36 | 219 | |
ODEM | 1:c53aafa72b36 | 220 | current_State = 2;// INITIALISIERUNG; |
ODEM | 1:c53aafa72b36 | 221 | } |
ODEM | 0:aaf583a75b97 | 222 | |
ODEM | 1:c53aafa72b36 | 223 | break; |
ODEM | 1:c53aafa72b36 | 224 | } |
ODEM | 1:c53aafa72b36 | 225 | |
ODEM | 1:c53aafa72b36 | 226 | |
ODEM | 0:aaf583a75b97 | 227 | // pc.printf("%d\n", MyEpos.GetError()); |
ODEM | 0:aaf583a75b97 | 228 | }//while |
ODEM | 0:aaf583a75b97 | 229 | }//main |