mmotor / Mbed 2 deprecated Projekt_Kugelbahn

2 Achsen Motorsteuerung mit Fehlerauswertung

Dependencies:   EPOS2 mbed-rtos mbed

main.cpp@0:aaf583a75b97, 2016-07-28 (annotated)

Committer:
ODEM
Date:
Thu Jul 28 06:24:33 2016 +0000
Revision:
0:aaf583a75b97
Child:
1:c53aafa72b36
error Auswertung funktioniert ausser Homing

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 0:aaf583a75b97 26 bool start = true;
ODEM 0:aaf583a75b97 27 bool counter_s =false;
ODEM 0:aaf583a75b97 28
ODEM 0:aaf583a75b97 29
ODEM 0:aaf583a75b97 30
ODEM 0:aaf583a75b97 31 void Motor1 (EPOS2 MyEpos){
ODEM 0:aaf583a75b97 32 pc.printf("Motor1\n");
ODEM 0:aaf583a75b97 33
ODEM 0:aaf583a75b97 34
ODEM 0:aaf583a75b97 35 MyEpos.SetDigOut(3, 0);
ODEM 0:aaf583a75b97 36 wait(2);
ODEM 0:aaf583a75b97 37
ODEM 0:aaf583a75b97 38
ODEM 0:aaf583a75b97 39
ODEM 0:aaf583a75b97 40
ODEM 0:aaf583a75b97 41 // while(1){
ODEM 0:aaf583a75b97 42
ODEM 0:aaf583a75b97 43
ODEM 0:aaf583a75b97 44 // pc.printf("start\n");
ODEM 0:aaf583a75b97 45 MyEpos.SetDigOut(3, 1); //(1=Enable, 0=Disable)
ODEM 0:aaf583a75b97 46 MyEpos.MoveAbsolute(500,20,500,deceleration);
ODEM 0:aaf583a75b97 47 MyEpos.SetDigOut(3, 0); //(TargetPosition, PosSpeed, acceleration, deceleration)
ODEM 0:aaf583a75b97 48 wait(1);
ODEM 0:aaf583a75b97 49 // pc.printf("%d\n", MyEpos.GetError());
ODEM 0:aaf583a75b97 50
ODEM 0:aaf583a75b97 51 while (MyEpos.TargetReached() == 0) {
ODEM 0:aaf583a75b97 52 ActualPos = MyEpos.GetActualPos();
ODEM 0:aaf583a75b97 53 DemandPos = MyEpos.GetDemandPos();
ODEM 0:aaf583a75b97 54 ActualCurrent = MyEpos.GetActualCurrentAveraged();
ODEM 0:aaf583a75b97 55 DemandCurrent = MyEpos.GetDemandCurrent();
ODEM 0:aaf583a75b97 56 //wait_ms(100);
ODEM 0:aaf583a75b97 57 MyEpos.SetDigOut(3, 0);
ODEM 0:aaf583a75b97 58 } // pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
ODEM 0:aaf583a75b97 59
ODEM 0:aaf583a75b97 60
ODEM 0:aaf583a75b97 61 wait(0.5);
ODEM 0:aaf583a75b97 62
ODEM 0:aaf583a75b97 63 MyEpos.SetDigOut(3, 1);
ODEM 0:aaf583a75b97 64 //wait_ms(100);
ODEM 0:aaf583a75b97 65 MyEpos.MoveAbsolute(0,3000,3000,3000);
ODEM 0:aaf583a75b97 66 MyEpos.SetDigOut(3, 0);
ODEM 0:aaf583a75b97 67 wait(1);
ODEM 0:aaf583a75b97 68
ODEM 0:aaf583a75b97 69
ODEM 0:aaf583a75b97 70 while (MyEpos.TargetReached() == 0) {
ODEM 0:aaf583a75b97 71 ActualPos = MyEpos.GetActualPos();
ODEM 0:aaf583a75b97 72 DemandPos = MyEpos.GetDemandPos();
ODEM 0:aaf583a75b97 73 ActualCurrent = MyEpos.GetActualCurrent();
ODEM 0:aaf583a75b97 74 DemandCurrent = MyEpos.GetDemandCurrent();
ODEM 0:aaf583a75b97 75 //wait_ms(100);
ODEM 0:aaf583a75b97 76 MyEpos.SetDigOut(3, 0);
ODEM 0:aaf583a75b97 77 //pc.printf("%10.d,%10.d,%10.hd,%10.hd\n", ActualPos, DemandPos, ActualCurrent, DemandCurrent);
ODEM 0:aaf583a75b97 78
ODEM 0:aaf583a75b97 79 }
ODEM 0:aaf583a75b97 80 //}
ODEM 0:aaf583a75b97 81 }
ODEM 0:aaf583a75b97 82
ODEM 0:aaf583a75b97 83
ODEM 0:aaf583a75b97 84 void Motor2(EPOS2 MyEpos2){
ODEM 0:aaf583a75b97 85 pc.printf("Motor2\n");
ODEM 0:aaf583a75b97 86
ODEM 0:aaf583a75b97 87
ODEM 0:aaf583a75b97 88
ODEM 0:aaf583a75b97 89 wait_ms(100) ;
ODEM 0:aaf583a75b97 90 MyEpos2. MoveVelocity(1000, 80, 1000);
ODEM 0:aaf583a75b97 91 wait(0.1);
ODEM 0:aaf583a75b97 92
ODEM 0:aaf583a75b97 93
ODEM 0:aaf583a75b97 94
ODEM 0:aaf583a75b97 95 }
ODEM 0:aaf583a75b97 96 int main() ///////// Main
ODEM 0:aaf583a75b97 97 {
ODEM 0:aaf583a75b97 98 restart:
ODEM 0:aaf583a75b97 99 //************** Initialisierung **************//
ODEM 0:aaf583a75b97 100 counter_s=false;
ODEM 0:aaf583a75b97 101 pc.baud(9600);
ODEM 0:aaf583a75b97 102 pc.printf("Fehlerfall %d\n", counter) ;
ODEM 0:aaf583a75b97 103 wait(2); //Wartezeit bis Epos2 70/10 aufgestartet ist
ODEM 0:aaf583a75b97 104
ODEM 0:aaf583a75b97 105 pc.printf("Initialisation CAN\n");
ODEM 0:aaf583a75b97 106
ODEM 0:aaf583a75b97 107 can.frequency(1000000); //Define Can baud in bit/s
ODEM 0:aaf583a75b97 108
ODEM 0:aaf583a75b97 109 CANopen canOpen(&can, 0.001); //Define CanOpen Network(can function, periode of the CANopen driver in sec)
ODEM 0:aaf583a75b97 110 canOpen.start(); //Start defined CanOpen Network
ODEM 0:aaf583a75b97 111
ODEM 0:aaf583a75b97 112 EPOS2 MyEpos(&canOpen, 1);
ODEM 0:aaf583a75b97 113 EPOS2 MyEpos2(&canOpen, 2); //Define and connect the EPOS2 to the CanOpen Network(canOpen function, Node ID)
ODEM 0:aaf583a75b97 114
ODEM 0:aaf583a75b97 115 //RobotControl robotControl(&MyEpos, &MyEpos2);
ODEM 0:aaf583a75b97 116 //robotControl.start();
ODEM 0:aaf583a75b97 117
ODEM 0:aaf583a75b97 118
ODEM 0:aaf583a75b97 119 MyEpos.SetPar(10,2660, 1, 3100, 2.76); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
ODEM 0:aaf583a75b97 120 MyEpos.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
ODEM 0:aaf583a75b97 121 MyEpos.Reset(); //Reset the EPOS2
ODEM 0:aaf583a75b97 122 wait(2);
ODEM 0:aaf583a75b97 123 MyEpos2.SetPar(10,936, 8, 1000, 29.8); //Set the motor parameter( Motortype, Current limit, Pole pairs, max velocity, thermal time constant winding)
ODEM 0:aaf583a75b97 124 MyEpos2.SetHomingPar(23, 10, 10, 0); //Set Homing Parameter (homing mode->35=Actual Position, speed, acceleration, offset)
ODEM 0:aaf583a75b97 125 MyEpos2.Reset(); //Reset the EPOS2
ODEM 0:aaf583a75b97 126 wait(2);
ODEM 0:aaf583a75b97 127 pc.printf("Homing\n");
ODEM 0:aaf583a75b97 128
ODEM 0:aaf583a75b97 129 MyEpos.Power(1);
ODEM 0:aaf583a75b97 130 pc.printf("power\n");
ODEM 0:aaf583a75b97 131 MyEpos2.Power(1); //Enable the Epos for Homing
ODEM 0:aaf583a75b97 132 pc.printf("Power2\n");
ODEM 0:aaf583a75b97 133 wait(2);
ODEM 0:aaf583a75b97 134 MyEpos.Homing(); //do Homing and wait untill homing is done // MyEpos.Power(1);
ODEM 0:aaf583a75b97 135 wait_ms(100);
ODEM 0:aaf583a75b97 136
ODEM 0:aaf583a75b97 137
ODEM 0:aaf583a75b97 138 pc.printf("Initialisation abgeschlossen\n");
ODEM 0:aaf583a75b97 139
ODEM 0:aaf583a75b97 140 while(1){
ODEM 0:aaf583a75b97 141
ODEM 0:aaf583a75b97 142 if (MyEpos.GetError()==true || MyEpos2.GetError()==true && counter<3) {
ODEM 0:aaf583a75b97 143 pc.printf("FEHLER!!!\n");
ODEM 0:aaf583a75b97 144
ODEM 0:aaf583a75b97 145 if (counter_s==false){
ODEM 0:aaf583a75b97 146 counter++;
ODEM 0:aaf583a75b97 147 counter_s=true;
ODEM 0:aaf583a75b97 148 }
ODEM 0:aaf583a75b97 149 goto restart;
ODEM 0:aaf583a75b97 150
ODEM 0:aaf583a75b97 151 }
ODEM 0:aaf583a75b97 152
ODEM 0:aaf583a75b97 153 if(counter>2){
ODEM 0:aaf583a75b97 154 MyEpos.Power(0);
ODEM 0:aaf583a75b97 155 MyEpos2.Power(0);
ODEM 0:aaf583a75b97 156 }
ODEM 0:aaf583a75b97 157
ODEM 0:aaf583a75b97 158 Motor2(MyEpos2);
ODEM 0:aaf583a75b97 159 Motor1(MyEpos);
ODEM 0:aaf583a75b97 160
ODEM 0:aaf583a75b97 161 // pc.printf("%d\n", MyEpos.GetError());
ODEM 0:aaf583a75b97 162 }//while
ODEM 0:aaf583a75b97 163 }//main