mmotor / Mbed 2 deprecated Projekt_Kugelbahn

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?

UserRevisionLine numberNew 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