//test on ZS-X12H
#define pinOdometry 2
#define pinMotorPWM 9
#define pinMotorBRK 10
#define pinMotorDIR 11
int odo_cible = 0; //
int motor_speed = 30; // from 0 to 255
int motor_brake_power = 50;
unsigned long odoLeft = 5000;
int odometryTicksPerRevolution = 510;
boolean usebrake = false;
boolean run_CCW = false;
boolean motor_stop =false;
unsigned long nextTimeOdometry ;
unsigned long lastMotorRpmTime ;
float odometryTicksPerCm ; // encoder ticks per cm
float motorLeftRpmCurr ; // left wheel rpm
float motorLeftSpeedRpmSet ; // left wheel rpm
void setup() {
motorLeftSpeedRpmSet = 30.0 ;
odometryTicksPerCm = 22 ; // encoder ticks per cm
nextTimeOdometry = millis(); ;
Serial.begin(115200);
Serial.println("Motor Test");
//analogWriteFreq(10000);
pinMode(pinMotorDIR, OUTPUT);
digitalWrite(pinMotorDIR, HIGH);
pinMode(pinMotorPWM, OUTPUT);
pinMode(pinMotorBRK, OUTPUT);
analogWrite(pinMotorPWM, 0);
pinMode(pinOdometry, INPUT); //plus/Hall sensor detection - Count steps
attachInterrupt(digitalPinToInterrupt(pinOdometry), plus, RISING);
int myEraser = 7; // this is 111 in binary and is used as an eraser
TCCR2B &= ~myEraser; // this operation (AND plus NOT), set the three bits in TCCR2B to 0
int myPrescaler = 2; // 1=31Khz 2=4Khz
TCCR2B |= myPrescaler;
delay(3000);
}
void plus() {
if (run_CCW) {
if (motor_stop){
odoLeft--; //count steps
}
else{
odoLeft++; //count steps
}
}
else {
odoLeft--; //count steps
}
}
// calculate map position by odometry sensors
void calcOdometry() {
if (millis() < nextTimeOdometry) return;
nextTimeOdometry = millis() + 100; //bb 300 at the original but test less
static int lastOdoLeft = 0;
int ticksLeft = odoLeft - lastOdoLeft;
lastOdoLeft = odoLeft;
double left_cm = ((double)ticksLeft) / ((double)odometryTicksPerCm);
motorLeftRpmCurr = abs(double ((( ((double)ticksLeft) / ((double)odometryTicksPerRevolution)) / ((double)(millis() - lastMotorRpmTime))) * 60000.0));
lastMotorRpmTime = millis();
if (run_CCW) {
if ((motorLeftRpmCurr) < motorLeftSpeedRpmSet) {
motor_speed = motor_speed + 2;
}
else {
motor_speed = motor_speed - 2;
}
}
else {
if ((motorLeftRpmCurr) < motorLeftSpeedRpmSet) {
motor_speed = motor_speed + 2;
motor_brake_power = motor_brake_power - 2;
if (motor_brake_power<0) motor_brake_power=0;
}
else {
motor_speed = motor_speed - 2;
motor_brake_power = motor_brake_power + 2;
}
if (motor_speed < 0) {
//digitalWrite(pinMotorBRK, LOW); //no brake
analogWrite(pinMotorBRK, motor_brake_power); //faster in load
}
else{
// analogWrite(pinMotorBRK, HIGH); //faster in load
analogWrite(pinMotorPWM, abs(motor_speed)); //faster in load
}
}
}
void loop() {
//on monte
motor_speed = 70; // from 0 to 1024
motor_stop=false;
run_CCW = true;
//Serial.print("On monte init : ");
//Serial.println(odoLeft);
odo_cible = 5500;
//odo_cible = odoLeft + odometryTicksPerRevolution;
digitalWrite(pinMotorBRK, HIGH); //no brake
digitalWrite(pinMotorDIR, HIGH);
analogWrite(pinMotorPWM, motor_speed); //faster in load
while (odoLeft < odo_cible) {
calcOdometry();
analogWrite(pinMotorPWM, motor_speed); //faster in load
Serial.print(motor_brake_power);
Serial.print(",");
Serial.print(motor_speed);
Serial.print(",");
Serial.print(motorLeftRpmCurr);
Serial.print(",");
Serial.print(odoLeft);
Serial.println();
}
analogWrite(pinMotorPWM, 0); //stop
digitalWrite(pinMotorBRK, LOW); // brake
motor_stop=true;
delay(2500);
//on descend
motor_speed = 3; // from 0 to 1024
motor_brake_power=70;
motor_stop=false;
//Serial.print("On descend : ");
//Serial.println(odoLeft);
odo_cible = 5000;
//odo_cible = odoLeft - odometryTicksPerRevolution;
run_CCW = false;
digitalWrite(pinMotorBRK, HIGH); //no brake
digitalWrite(pinMotorDIR, LOW);
analogWrite(pinMotorPWM, motor_speed); //faster in load
while (odoLeft >= odo_cible) {
calcOdometry();
analogWrite(pinMotorPWM, abs(motor_speed)); //faster in load
Serial.print(motor_brake_power);
Serial.print(",");
Serial.print(motor_speed);
Serial.print(",");
Serial.print(motorLeftRpmCurr);
Serial.print(",");
Serial.print(odoLeft);
Serial.println();
}
analogWrite(pinMotorPWM, 0); //stop
digitalWrite(pinMotorBRK, LOW); // brake
motor_stop=true;
delay(2500);
}
