Attempted fix of DMOC645 to run

DMOC.txt

@@ -0,0 +1,72 @@
+This document explains a little bit of theory about the DMOC645 
+motorcontroller object-module.
+
+The DMOC645 is a bit complicated in that you need to progressively establish
+operation in either speed mode or torque mode.  We really don't use speed mode
+in vehicle applications.  
+
+We control DMOC operation through two main variables and a torque command generated from our throttle:
+
+operationState=	DISABLED = 0,
+				STANDBY = 1,
+				ENABLE = 2,
+				POWERDOWN = 3
+
+selectedGear  NEUTRAL =0
+			  DRIVE =1
+			  REVERSE=2
+			  ERROR=3
+
+
+The problem is, we have to allow some time for communications to establish, and then 
+we have to progressively cycle from DISABLED to STANDBY to ENABLE.  And we need acknowledgement from 
+DMOC at each step.
+
+DMOC reports its state of operation via CAN and we hold that state in a variable titled activeState.
+
+We initialize as:
+  selectedGear = NEUTRAL;
+	operationState = DISABLED;
+	actualState = DISABLED;
+
+We can set these values programmatically using two functions provided by our parent class MotorController.  They are setOpState(operationState) and setSelectedGear(selectedGear).
+
+We can also set them via hardware input by activating the ENABLE input and the REVERSE input to any of the four digital inputs.
+When 12v is applied to the input designated as ENABLE, it will call setOpState(ENABLE).  Likewise a 12v on the REVERSE input will cause a setSelectedGear(REVERSE) call.  When they are in a low state, they actively setSelectedGear(DRIVE) and setOpState(DISABLED)
+
+
+The DMOC requires three different frames twice per second beginning very soon after power up or otherwise it faults out.
+
+We hold these in SENDCMD1, SENDCMD2, and SENDCMD3.
+
+SENDCMD1 sends a 0x232 frame we think of as the speed control frame.  It is in speed mode.  But we MUST transmit this frame even in torque mode.  BYTE 5 is actually our ignition key state to get this thing running.  Byte 6 is a bit map containing an ALIVE counter that increments by 2 from 0x00 to 0x0F.  It also contains our selected gear and our requested operation state.  We use the variable NEWSTATE for this.  
+
+Examining our logic, if the DMOC reports, as it will initially, an activeState of DISABLED, if our operationState is DISABLED we will leave it there and in fact indicate a NEUTRAL gear selection as well, regardless of selectedGear.
+
+If DMOC reports DISABLED and we have an operationState of either STANDBY or ENABLE, we will set newstate to STANDBY and send that to the DMOC.  This will continue UNTIL we receive an activeState BACK from the DMOC indicating STANDBY.
+
+Once we receive an actualState of STANDBY from the DMOC, on our next transmitted 232 frame, if our operationState is ENABLE we will send a newstate of ENABLE. 
+
+Thereafter, if we receive an activeState indication from DMOC of ENABLE and our operationState is ENABLE, we continue to send ENABLE.
+
+Of course, if operationState goes to DISABLED, we will send a DISABLED to the DMOC and have to start the sequence all over again if we return to ENABLE on the operationState.
+
+IF we have a newstate of ENABLE, we also send our selectedGear instead of NEUTRAL.  So the SpeedControl frame 232 is actually key to properly cycling through the DISABLED, STANDBY, and ENABLE states AND advising DMOC of our selectedGear.
+
+SENDCMD2 sends a 233 frame containing our torque command in bytes 0 and 1 and again in 2 and 3.  It sends a STANDBy torque command in bytes 4 and 5.  Byte 6 contains the SAME alive value as the 232 frame.
+
+Our torque command is calculated by multiplying our maximum allowed torque variable by our throttle percentage.  Throttle runs from -1000 to +1000 as a percentage and is calculated by comparing the received analog values in the range of x1 to x2 to our throttle map of regen and forward torques.  We multiply our throttle position x maximum torque and divide by 1000 to get a torque value that is a percentage of maxium torque, but has the sign of regen (-) or forward(+).  This is added to an offset of 30,000.  Numbers below 30000 are regen torque and above 30000 are forward torque.
+
+The problem comes in in REVERSE gear.  DMOC takes negative values as positive torque in the reverse direction.  And it takes positive values as regen in the reverse direction.  By inverting the value, we can provide control in a reversed direction.
+
+
+And so in normal operation, we can set our selectedGear and operationState at will, and the proper cycling will occur within these two frames automatically.
+
+The only issue is on startup.  We want to wait until we have received and sent some frames to establish communications before doing all this.  And so we have an activity counter that is incremented by frames received, and decremented by our tick handler.  Once we have accumulated a surplus of 40 on the activity counter, we can set our gear to DRIVE and our opstate to ENABLE.
+
+IF we have selected an input for either REVERSE or ENABlE, we don't want to do that.  We want to leave it in neutral and disabled and let this be done by hardware signal input.  If we have NOT selected inputs for these, we of course want to go ahead and set them automatically.
+
+The RUNNING advisory on our web status panel only appears on receipt of DMOC CAN packets.  In fact, if we lose our stream of packets and our activity counter is decremented below 40, it will go out as well.
+
+
+

DmocMotorController.cpp

@@ -50,11 +50,10 @@ DmocMotorController::DmocMotorController() : MotorController() {
 	prefsHandler = new PrefHandler(DMOC645);
 	step = SPEED_TORQUE;
 
-        selectedGear = NEUTRAL;
+    selectedGear = NEUTRAL;
 	operationState = DISABLED;
 	actualState = DISABLED;
 	online = 0;
-	wentEnabled = false;
 	activityCount = 0;
 //	maxTorque = 2000;
 	commonName = "DMOC645 Inverter";
@@ -72,9 +71,9 @@ void DmocMotorController::setup() {
 	CanHandler::getInstanceEV()->attach(this, 0x230, 0x7f0, false);
 	CanHandler::getInstanceEV()->attach(this, 0x650, 0x7f0, false);
 
-	actualGear = NEUTRAL;
-	running = true;
+	running = false;
 	setPowerMode(modeTorque);
+
 
 	TickHandler::getInstance()->attach(this, CFG_TICK_INTERVAL_MOTOR_CONTROLLER_DMOC);
 }
@@ -91,7 +90,10 @@ void DmocMotorController::handleCanFrame(CAN_FRAME *frame) {
 	int RotorTemp, invTemp, StatorTemp;
 	int temp;
 	online = 1; //if a frame got to here then it passed the filter and must have been from the DMOC
-        //Logger::debug("dmoc msg: %i", frame->id);
+
+        //Logger::debug("DMOC CAN received: %X  %X  %X  %X  %X  %X  %X  %X  %X", frame->id,frame->data.bytes[0] ,frame->data.bytes[1],frame->data.bytes[2],frame->data.bytes[3],frame->data.bytes[4],frame->data.bytes[5],frame->data.bytes[6],frame->data.bytes[70]);
+
+
 	switch (frame->id) {
 	case 0x651: //Temperature status
 		RotorTemp = frame->data.bytes[0];
@@ -111,44 +113,63 @@ void DmocMotorController::handleCanFrame(CAN_FRAME *frame) {
 		torqueActual = ((frame->data.bytes[0] * 256) + frame->data.bytes[1]) - 30000;
 		activityCount++;
 		break;
+
 	case 0x23B: //speed and current operation status
 		speedActual = ((frame->data.bytes[0] * 256) + frame->data.bytes[1]) - 20000;
 		temp = (OperationState) (frame->data.bytes[6] >> 4);
 		//actually, the above is an operation status report which doesn't correspond
 		//to the state enum so translate here.
 		switch (temp) {
+
 		case 0: //Initializing
 			actualState = DISABLED;
+			faulted=false;
 			break;
+
 		case 1: //disabled
 			actualState = DISABLED;
+			faulted=false;
 			break;
+
 		case 2: //ready (standby)
 			actualState = STANDBY;
+			faulted=false;
 			ready = true;
 			break;
+
 		case 3: //enabled
 			actualState = ENABLE;
+			faulted=false;
 			break;
+
 		case 4: //Power Down
 			actualState = POWERDOWN;
+			faulted=false;
 			break;
+
 		case 5: //Fault
 			actualState = DISABLED;
+			faulted=true;
 			break;
+
 		case 6: //Critical Fault
 			actualState = DISABLED;
+			faulted=true;
 			break;
+
 		case 7: //LOS
 			actualState = DISABLED;
+			faulted=true;
 			break;
 		}
-//      Logger::debug("OpState: %d", temp);
+      Logger::debug("OpState: %d", temp);
 		activityCount++;
 		break;
+
 	//case 0x23E: //electrical status
 		//gives volts and amps for D and Q but does the firmware really care?
 		//break;
+
 	case 0x650: //HV bus status
 		dcVoltage = ((frame->data.bytes[0] * 256) + frame->data.bytes[1]);
 		dcCurrent = ((frame->data.bytes[2] * 256) + frame->data.bytes[3]) - 5000; //offset is 500A, unit = .1A
@@ -167,25 +188,26 @@ void DmocMotorController::handleTick() {
 	if (activityCount > 0) {
 		activityCount--;
 		if (activityCount > 60) activityCount = 60;
-		if (actualState == DISABLED && activityCount > 40 && !wentEnabled) {
-			if (getEnableIn() >= 0 && getEnableIn() < 4) setOpState(ENABLE);
-			if (getReverseIn() >= 0 && getReverseIn() < 4)setGear(DRIVE);
-			wentEnabled = true;
-		}
-	}
+		if (activityCount > 40) //If we are receiving regular CAN messages from DMOC, this will very quickly get to over 40. We'll limit
+								// it to 60 so if we lose communications, within 20 ticks we will decrement below this value.
+			{
+			if(getEnableIn()>4)setOpState(ENABLE); //If we HAVE an enableinput 0-3, we'll let that handle opstate. Otherwise set it to ENABLE
+			if(getReverseIn()>4)setSelectedGear(DRIVE); //If we HAVE a reverse input, we'll let that determine forward/reverse.  Otherwise set it to DRIVE
+			running=true; //Lets's set this on as long as we are 40 frames ahead.   
+		    }
+	  }
 	else {
-		setGear(NEUTRAL);
-		wentEnabled = false;
+		setSelectedGear(NEUTRAL); //We will stay in NEUTRAL until we get at least 40 frames ahead indicating continous communications.
+		running=false;
 	}
 
-	//if (online == 1) { //only send out commands if the controller is really there.
-	step = CHAL_RESP;
-	sendCmd1();
-	sendCmd2();
+
+
+	sendCmd1();  //This actually sets our GEAR and our actualstate cycle
+	sendCmd2();  //This is our torque command
 	sendCmd3();
-	//sendCmd4();
-	//sendCmd5();
-	//}
+	//sendCmd4();  //These appear to be not needed.
+	//sendCmd5();  //But we'll keep them for future reference
 
 
 }
@@ -201,7 +223,7 @@ void DmocMotorController::sendCmd1() {
 	output.extended = 0; //standard frame
 	output.rtr = 0;
 
-	if (throttleRequested > 0 && operationState == ENABLE && selectedGear != NEUTRAL && config->motorMode == modeSpeed)
+	if (throttleRequested > 0 && operationState == ENABLE && selectedGear != NEUTRAL && powerMode == modeSpeed)
 		speedRequested = 20000 + (((long) throttleRequested * (long) config->speedMax) / 1000);
 	else
 		speedRequested = 20000;
@@ -248,13 +270,13 @@ void DmocMotorController::sendCmd2() {
 	//torqueRequested = 30000L + (((long)throttleRequested * (long)MaxTorque) / 1000L);
 
 	torqueCommand = 30000; //set offset  for zero torque commanded
-	if (config->motorMode == modeTorque) {
-                torqueRequested=0;
+
+        torqueRequested=0;
 		if (actualState == ENABLE) { //don't even try sending torque commands until the DMOC reports it is ready
 			if (selectedGear == DRIVE)
-                torqueRequested = (((long) throttleRequested * (long) config->torqueMax) / 1000L);
+                            torqueRequested = (((long) throttleRequested * (long) config->torqueMax) / 1000L);
 			if (selectedGear == REVERSE)
-			    torqueRequested = (((long) throttleRequested * (long) config->torqueMax) / 1000L);
+			    torqueRequested = (((long) throttleRequested * -1 *(long) config->torqueMax) / 1000L);//If reversed, regen becomes positive torque and positive pedal becomes regen.  Let's reverse this by reversing the sign.  In this way, we'll have gradually diminishing positive torque (in reverse, regen) followed by gradually increasing regen (positive torque in reverse.)
 		}
 
                 if(speedActual < config->speedMax){torqueCommand+=torqueRequested;} //If actual rpm is less than max rpm, add torque to offset
@@ -263,14 +285,7 @@ void DmocMotorController::sendCmd2() {
 		output.data.bytes[1] = (torqueCommand & 0x00FF);
 		output.data.bytes[2] = output.data.bytes[0];
 		output.data.bytes[3] = output.data.bytes[1];
-	}
-	else { //RPM mode so request max torque as upper limit and zero torque as lower limit
-		output.data.bytes[0] = ((30000L + config->torqueMax) & 0xFF00) >> 8;
-		output.data.bytes[1] = ((30000L + config->torqueMax) & 0x00FF);
-		output.data.bytes[2] = 0x75;
-		output.data.bytes[3] = 0x30;
-	}
-
+
 	//what the hell is standby torque? Does it keep the transmission spinning for automatics? I don't know.
 	output.data.bytes[4] = 0x75; //msb standby torque. -3000 offset, 0.1 scale. These bytes give a standby of 0Nm
 	output.data.bytes[5] = 0x30; //lsb

DmocMotorController.h

@@ -74,16 +74,15 @@ class DmocMotorController: public MotorController, CanObserver {
 	virtual void saveConfiguration();
 
 private:
-	Gears actualGear;
+
 	OperationState actualState; //what the controller is reporting it is
 	int step;
 	byte online; //counter for whether DMOC appears to be operating
 	byte alive;
 	int activityCount;
-    uint16_t torqueCommand;
-	bool wentEnabled;
-
-	void timestamp();
+        uint16_t torqueCommand;
+        void timestamp();
+
 	void sendCmd1();
 	void sendCmd2();
 	void sendCmd3();

MotorController.cpp

@@ -97,17 +97,10 @@ void MotorController::handleTick() {
      //Throttle check
 	Throttle *accelerator = DeviceManager::getInstance()->getAccelerator();
 	Throttle *brake = DeviceManager::getInstance()->getBrake();
-
-	throttleRequested = 0;
-
 	if (accelerator)
-	{
 		throttleRequested = accelerator->getLevel();
-	}
 	if (brake && brake->getLevel() < -10 && brake->getLevel() < accelerator->getLevel()) //if the brake has been pressed it overrides the accelerator.
-	{
 		throttleRequested = brake->getLevel();
-	}
 	//Logger::debug("Throttle: %d", throttleRequested);
 
 
@@ -125,19 +118,19 @@ void MotorController::handleTick() {
 		    dcVoltage--;  
 	            if (torqueActual < -500)
                       {
-                        torqueActual=600;
+                        torqueActual=20;
                       }
                      else 
                        {
                          torqueActual=-650;
                        }
                         if (dcCurrent < 0)
                       {
-                        dcCurrent=1200;
+                        dcCurrent=120;
                       }
                      else 
                        {
-                         dcCurrent=-650;
+                         dcCurrent=-65;
                        }
                     if (temperatureInverter < config->coolOn*10)
                       {
@@ -150,7 +143,7 @@ void MotorController::handleTick() {
 
                     if (throttleRequested < 500)
                       {
-                        throttleRequested=2000;
+                        throttleRequested=500;
                       }
                       else 
                       {
@@ -237,7 +230,7 @@ void MotorController::coolingcheck()
  {
 	int coolfan=getCoolFan();
 
-	if(coolfan>=0 && coolfan<8)    //We have 8 outputs 0-7 If they entered something else, there is no point in doing this check.
+	if(coolfan>=0 and coolfan<8)    //We have 8 outputs 0-7 If they entered something else, there is no point in doing this check.
 	  {          
 	    if(temperatureInverter/10>getCoolOn())  //If inverter temperature greater than COOLON, we want to turn on the coolingoutput
 	      {
@@ -361,11 +354,12 @@ void MotorController::setup() {
 
 
     if(config->prechargeR==12345)
-    {  
-		torqueActual=2;
+      {  
+	torqueActual=2;
         dcCurrent=1501;
         dcVoltage=3320;
-    }
+
+      }
 
     Logger::console("PRELAY=%i - Current PreCharge Relay output", config->prechargeRelay);
     Logger::console("MRELAY=%i - Current Main Contactor Relay output", config->mainContactorRelay);
@@ -575,7 +569,7 @@ void MotorController::loadConfiguration() {
 		prefsHandler->read(EEMC_REV_LIGHT, &config->revLight);
 		prefsHandler->read(EEMC_ENABLE_IN, &config->enableIn);
 		prefsHandler->read(EEMC_REVERSE_IN, &config->reverseIn);
-		prefsHandler->read(EEMC_MOTOR_MODE, (uint8_t *)&config->motorMode);
+
 	}
 	else { //checksum invalid. Reinitialize values and store to EEPROM
 		config->speedMax = MaxRPMValue;
@@ -595,7 +589,6 @@ void MotorController::loadConfiguration() {
 		config->revLight = RevLight;
 		config->enableIn = EnableIn;
 		config->reverseIn = ReverseIn;
-		config->motorMode = MotorController::modeSpeed;
 
 	}
            //DeviceManager::getInstance()->sendMessage(DEVICE_WIFI, ICHIP2128, MSG_CONFIG_CHANGE, NULL);
@@ -625,7 +618,7 @@ void MotorController::saveConfiguration() {
 	prefsHandler->write(EEMC_REV_LIGHT, config->revLight);
 	prefsHandler->write(EEMC_ENABLE_IN, config->enableIn);
 	prefsHandler->write(EEMC_REVERSE_IN, config->reverseIn);
-	prefsHandler->write(EEMC_MOTOR_MODE, (uint8_t)config->motorMode);
+
 
 	prefsHandler->saveChecksum();
 	loadConfiguration();