Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Servo.h>
- #include "IOpins.h"
- #include "Constants.h"
- //-------------------------------------------------------------- define global variables ---------------------
- unsigned int Volts;
- unsigned int LeftAmps;
- unsigned int RightAmps;
- unsigned long chargeTimer;
- unsigned long leftoverload;
- unsigned long rightoverload;
- int highVolts;
- int startVolts;
- int Leftspeed=0;
- int Rightspeed=0;
- int Speed;
- int Steer;
- byte Charged=1; // 0=Flat battery 1=Charged battery
- int Leftmode=1; // 0=reverse, 1=brake, 2=forward
- int Rightmode=1; // 0=reverse, 1=brake, 2=forward
- byte Leftmodechange=0; // Left input must be 1500 before brake or reverse can occur
- byte Rightmodechange=0; // Right input must be 1500 before brake or reverse can occur
- int LeftPWM; // PWM value for left motor speed / brake
- int RightPWM; // PWM value for right motor speed / brake
- int data;
- int servo[7];
- const int trigger = 2; // aaa Define Ping Trigger as D2
- const int echo = 4; // aaa Define Ping Echo D4
- //-------------------------------------------------------------- define servos --------------------------------
- Servo Servo0; // define servos
- Servo Servo1; // define servos
- Servo Servo2; // define servos
- Servo Servo3; // define servos
- Servo Servo4; // define servos
- Servo Servo5; // define servos
- Servo Servo6; // define servos
- void setup()
- {
- //------------------------------------------------------------ Initialize Servos ------------------------------
- //Servo0.attach(S0); // attach servo to I/O pin
- //Servo1.attach(S1); // attach servo to I/O pin
- Servo2.attach(S2); // attach servo to I/O pin
- Servo3.attach(S3); // attach servo to I/O pin
- Servo4.attach(S4); // attach servo to I/O pin
- Servo5.attach(S5); // attach servo to I/O pin
- Servo6.attach(S6); // attach servo to I/O pin
- //------------------------------------------------------------ Set servos to default position -----------------
- Servo0.writeMicroseconds(DServo0); // set servo to default position
- Servo1.writeMicroseconds(DServo1); // set servo to default position
- Servo2.writeMicroseconds(DServo2); // set servo to default position
- Servo3.writeMicroseconds(DServo3); // set servo to default position
- Servo4.writeMicroseconds(DServo4); // set servo to default position
- Servo5.writeMicroseconds(DServo5); // set servo to default position
- Servo6.writeMicroseconds(DServo6); // set servo to default position
- //------------------------------------------------------------ Initialize I/O pins ---------------------------
- pinMode (Charger,OUTPUT); // change Charger pin to output
- digitalWrite (Charger,1); // disable current regulator to charge battery
- if (Cmode==1)
- {
- Serial.begin(Brate); // enable serial communications if Cmode=1
- Serial.flush(); // flush buffer
- }
- //Serial.begin(57600);
- //------------------------------------------------------------ Initilialize I/O Ping Sensor -----------------
- pinMode(trigger,OUTPUT); // aaa Set digital pin 2 to trigger output
- pinMode(echo,INPUT); // aaa Set digital pin 4 to echo input
- }
- void loop()
- {
- //---------------------------------------- Check battery voltage and current draw of motors ---------------
- Volts=analogRead(Battery); // read the battery voltage
- LeftAmps=analogRead(LmotorC); // read left motor current draw
- RightAmps=analogRead(RmotorC); // read right motor current draw
- //Serial.print(LeftAmps);
- //Serial.print(" ");
- //Serial.println(RightAmps);
- if (LeftAmps>Leftmaxamps) // is motor current draw exceeding safe limit
- {
- analogWrite (LmotorA,0); // turn off motors
- analogWrite (LmotorB,0); // turn off motors
- leftoverload=millis(); // record time of overload
- }
- if (RightAmps>Rightmaxamps) // is motor current draw exceeding safe limit
- {
- analogWrite (RmotorA,0); // turn off motors
- analogWrite (RmotorB,0); // turn off motors
- rightoverload=millis(); // record time of overload
- }
- if ((Volts<lowvolt) && (Charged==1)) // check condition of the battery
- { // change battery status from charged to flat
- //-------------------------- FLAT BATTERY speed controller shuts down until battery is recharged ----
- //--------------------------- This is a safety feature to prevent malfunction at low voltages!! ------
- Charged=0; // battery is flat
- highVolts=Volts; // record the voltage
- startVolts=Volts;
- chargeTimer=millis(); // record the time
- digitalWrite (Charger,0); // enable current regulator to charge battery
- }
- //------------------------------- CHARGE BATTERY -------------------------------------------------------
- if ((Charged==0) && (Volts-startVolts>67)) // if battery is flat and charger has been connected (voltage has increased by at least 1V)
- {
- if (Volts>highVolts) // has battery voltage increased?
- {
- highVolts=Volts; // record the highest voltage. Used to detect peak charging.
- chargeTimer=millis(); // when voltage increases record the time
- }
- if (Volts>batvolt) // battery voltage must be higher than this before peak charging can occur.
- {
- if ((highVolts-Volts)>5 || (millis()-chargeTimer)>chargetimeout) //has voltage begun to drop or levelled out?
- {
- Charged=1; // battery voltage has peaked
- digitalWrite (Charger,1); // turn off current regulator
- }
- }
- }
- else
- {//---------------------- GOOD BATTERY speed controller opperates normally ----------------------
- switch(Cmode)
- {
- case 0: // RC mode via D0 and D1
- RCmode();
- break;
- case 1: // Serial mode via D0(RX) and D1(TX)
- SCmode();
- break;
- case 2: // I2C mode via A4(SDA) and A5(SCL)
- I2Cmode();
- break;
- }
- // ------------------------------ Code to drive dual "H" bridges --------------------------------------
- if (Charged==1) // Only power motors if battery voltage is good
- {
- if ((millis()-leftoverload)>overloadtime)
- {
- switch (Leftmode) // if left motor has not overloaded recently
- {
- case 2: // left motor forward (2 = forward)
- analogWrite(LmotorA,0);
- analogWrite(LmotorB,LeftPWM);
- break;
- case 1: // left motor brake (1 = brake)
- analogWrite(LmotorA,LeftPWM);
- analogWrite(LmotorB,LeftPWM);
- break;
- case 0: // left motor reverse (0 = reverse)
- analogWrite(LmotorA,LeftPWM);
- analogWrite(LmotorB,0);
- break;
- }
- }
- if ((millis()-rightoverload)>overloadtime)
- {
- switch (Rightmode) // if right motor has not overloaded recently
- {
- case 2: // right motor forward
- analogWrite(RmotorA,0);
- analogWrite(RmotorB,RightPWM);
- break;
- case 1: // right motor brake
- analogWrite(RmotorA,RightPWM);
- analogWrite(RmotorB,RightPWM);
- break;
- case 0: // right motor reverse
- analogWrite(RmotorA,RightPWM);
- analogWrite(RmotorB,0);
- break;
- }
- }
- }
- else // Battery is flat
- {
- analogWrite (LmotorA,0); // turn off motors
- analogWrite (LmotorB,0); // turn off motors
- analogWrite (RmotorA,0); // turn off motors
- analogWrite (RmotorB,0); // turn off motors
- }
- }
- }
- void RCmode()
- {
- //---------------------------- Code for RC inputs ---------------------------------------------------------
- Speed=pulseIn(RCleft,HIGH,25000); // read throttle/left stick
- Steer=pulseIn(RCright,HIGH,25000); // read steering/right stick
- if (Speed==0) Speed=1500; // if pulseIn times out (25mS) then set speed to stop
- if (Steer==0) Steer=1500; // if pulseIn times out (25mS) then set steer to centre
- if (abs(Speed-1500)<RCdeadband) Speed=1500; // if Speed input is within deadband set to 1500 (1500uS=center
- // position for most servos)
- if (abs(Steer-1500)<RCdeadband) Steer=1500; // if Steer input is within deadband set to 1500
- //(1500uS=center position for most servos)
- if (Mix==1) // Mixes speed and steering signals
- {
- Steer=Steer-1500;
- Leftspeed=Speed-Steer;
- Rightspeed=Speed+Steer;
- }
- else // Individual stick control
- {
- Leftspeed=Speed;
- Rightspeed=Steer;
- }
- /*
- Serial.print("Left:");
- Serial.print(Leftspeed);
- Serial.print(" -- Right:");
- Serial.println(Rightspeed);
- */
- Leftmode=2;
- Rightmode=2;
- if (Leftspeed>(Leftcenter+RCdeadband)) Leftmode=0; // if left input is forward then set left mode to forward
- if (Rightspeed>(Rightcenter+RCdeadband)) Rightmode=0; // if right input is forward then set right mode to forward
- LeftPWM=abs(Leftspeed-Leftcenter)*10/scale; // scale 1000-2000uS to 0-255
- LeftPWM=min(LeftPWM,255); // set maximum limit 255
- RightPWM=abs(Rightspeed-Rightcenter)*10/scale; // scale 1000-2000uS to 0-255
- RightPWM=min(RightPWM,255); // set maximum limit 255
- }
- void SCmode()
- {// ------------------------------------------------------------ Code for Serial Communications --------------------------------------
- // FL = flush serial buffer
- // AN = report Analog inputs 1-5
- // SV = next 7 integers will be position information for servos 0-6
- // HB = "H" bridge data - next 4 bytes will be:
- // left motor mode 0-2
- // left motor PWM 0-255
- // right motor mode 0-2
- // right motor PWM 0-255
- if (Serial.available()>1) // command available
- {
- int A=Serial.read();
- int B=Serial.read();
- int command=A*256+B;
- switch (command)
- {
- case 17996: // FL
- Serial.flush(); // flush buffer
- break;
- case 16718: // AN - return values of analog inputs 1-5
- for (int i=1;i<6;i++) // index analog inputs 1-5
- {
- data=analogRead(i); // read 10bit analog input
- Serial.write(highByte(data)); // transmit high byte
- Serial.write(lowByte(data)); // transmit low byte
- }
- break;
- case 21334: // SV - receive postion information for servos 0-6
- for (int i=0;i<15;i++) // read 14 bytes of data
- {
- Serialread();
- servo[i]=data;
- }
- Servo0.writeMicroseconds(servo[0]*256+servo[1]); // set servo position
- Servo1.writeMicroseconds(servo[2]*256+servo[3]); // set servo position
- Servo2.writeMicroseconds(servo[4]*256+servo[5]); // set servo position
- Servo3.writeMicroseconds(servo[6]*256+servo[7]); // set servo position
- Servo4.writeMicroseconds(servo[8]*256+servo[9]); // set servo position
- Servo5.writeMicroseconds(servo[10]*256+servo[11]); // set servo position
- Servo6.writeMicroseconds(servo[12]*256+servo[13]); // set servo position
- break;
- case 18498: // HB - mode and PWM data for left and right motors
- Serialread();
- Leftmode=data;
- Serialread();
- LeftPWM=data;
- Serialread();
- Rightmode=data;
- Serialread();
- RightPWM=data;
- break;
- default: // invalid command
- Serial.flush(); // flush buffer
- }
- }
- }
- void Serialread()
- {//------------------ Read serial port until data has been received -----------------------------------
- do
- {
- data=Serial.read();
- } while (data<0);
- }
- void I2Cmode()
- {//----------------------- Your code goes here ------------------------------------------------------
- void loop(){
- RightPWM = 128; // sets the speed to half full speed
- LeftPWM = 128;
- Rightmode = 2; // sets the right motor to move forward (at half speed)
- Leftmode = 2; // see code to drive dual H bridges
- unsigned long duration, cm;
- digitalWrite(trigger, LOW); // Ensures a clean trigger pulse
- delayMicroseconds(5); // .1 ms long cleaning
- digitalWrite(trigger, HIGH); // Turn trigger pin on high (5V) to initiate pulse
- delayMicroseconds(10); // delay 10 us (minimum time to start pulse)
- digitalWrite(trigger, LOW); // stop tigger pulse to listen to echo
- duration = pulseIn(echo, HIGH); // records the length of the echo pulse
- cm = duration/58; // converts duration into centimeters
- if (cm < 100);{ // is there an obstacle < 100cm away?
- RightPWM = 256; // set right motor full speed
- LeftPWM = 0; // set left motor to 0, therefore turning left
- delay(3000); // allow 1s for turn to complete
- RightPWM = 0;
- LeftPWM = 256; // turn right (straighten back out)
- delay(3000);
- RightPWM = 128; // back to half speed forward
- LeftPWM = 128;
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement